diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 03bd7c7570..66a938a9a2 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -13,7 +13,6 @@ pipeline { arm64: "px4io/px4-dev-aarch64:2021-05-04", base: "px4io/px4-dev-base-bionic:2021-05-04", nuttx: "px4io/px4-dev-nuttx-focal:2021-05-04", - snapdragon: "lorenzmeier/px4-dev-snapdragon:2020-04-01" ] def armhf_builds = [ @@ -106,14 +105,8 @@ pipeline { archive: true ] - def snapdragon_builds = [ - target: ["atlflight_eagle_qurt", "atlflight_eagle_default"], - image: docker_images.snapdragon, - archive: false - ] - def docker_builds = [ - armhf_builds, base_builds, nuttx_builds_archive//, snapdragon_builds + armhf_builds, base_builds, nuttx_builds_archive ] for (def build_type = 0; build_type < docker_builds.size(); build_type++) { @@ -165,12 +158,6 @@ pipeline { def createBuildNode(Boolean archive, String docker_image, String target) { return { - // TODO: fix the snapdragon image - bypass_entrypoint = '' - if (docker_image == 'lorenzmeier/px4-dev-snapdragon:2020-04-01') { - bypass_entrypoint = ' --entrypoint=""' - } - node { docker.withRegistry('https://registry.hub.docker.com', 'docker_hub_dagar') { docker.image(docker_image).inside('-e CCACHE_BASEDIR=${WORKSPACE} -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + bypass_entrypoint) { diff --git a/.gitmodules b/.gitmodules index 4b7e942c65..131e3a40e1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,10 +18,6 @@ path = src/lib/matrix url = https://github.com/PX4/PX4-Matrix.git branch = master -[submodule "boards/atlflight/cmake_hexagon"] - path = boards/atlflight/cmake_hexagon - url = https://github.com/PX4/cmake_hexagon.git - branch = px4 [submodule "src/drivers/gps/devices"] path = src/drivers/gps/devices url = https://github.com/PX4/PX4-GPSDrivers.git @@ -38,9 +34,6 @@ path = platforms/nuttx/NuttX/apps url = https://github.com/PX4/NuttX-apps.git branch = px4_firmware_nuttx-10.0.0+ -[submodule "platforms/qurt/dspal"] - path = platforms/qurt/dspal - url = https://github.com/ATLFlight/dspal.git [submodule "Tools/flightgear_bridge"] path = Tools/flightgear_bridge url = https://github.com/PX4/PX4-FlightGear-Bridge.git diff --git a/Makefile b/Makefile index bf7f645158..0c4c2a49a7 100644 --- a/Makefile +++ b/Makefile @@ -214,7 +214,7 @@ define colorecho endef # Get a list of all config targets boards/*/*.cmake -ALL_CONFIG_TARGETS := $(shell find boards -maxdepth 3 -mindepth 3 ! -name '*common*' ! -name '*sdflight*' -name '*.cmake' -print | sed -e 's|boards\/||' | sed -e 's|\.cmake||' | sed -e 's|\/|_|g' | sort) +ALL_CONFIG_TARGETS := $(shell find boards -maxdepth 3 -mindepth 3 -name '*.cmake' -print | sed -e 's|boards\/||' | sed -e 's|\.cmake||' | sed -e 's|\/|_|g' | sort) # ADD CONFIGS HERE # -------------------------------------------------------------------- @@ -240,16 +240,6 @@ endef # All targets with just dependencies but no recipe must either be marked as phony (or have the special @: as recipe). .PHONY: all px4_sitl_default all_config_targets all_default_targets -# Multi- config targets. -eagle_default: atlflight_eagle_default atlflight_eagle_qurt -eagle_rtps: atlflight_eagle_rtps atlflight_eagle_qurt-rtps - -excelsior_default: atlflight_excelsior_default atlflight_excelsior_qurt -excelsior_rtps: atlflight_excelsior_rtps atlflight_excelsior_qurt-rtps - -.PHONY: eagle_default eagle_rtps -.PHONY: excelsior_default excelsior_rtps - # Other targets # -------------------------------------------------------------------- @@ -512,7 +502,7 @@ help: @echo "Where is one of:" @$(MAKE) -pRrq -f $(lastword $(MAKEFILE_LIST)) : 2>/dev/null | \ awk -v RS= -F: '/^# File/,/^# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ - egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(posix|eagle|Makefile)' + egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(Makefile)' @echo @echo "Or, $(MAKE) []" @echo "Use '$(MAKE) list_config_targets' for a list of configuration targets." diff --git a/README.md b/README.md index 8ded0817b6..ed6cf464d6 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones. -PX4 is highly portable, OS-independent and supports Linux, NuttX and QuRT out of the box. +PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box. * Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/master/LICENSE)) * [Supported airframes](https://docs.px4.io/master/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)): diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index 9340b3a36b..cb8d903802 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -11,9 +11,6 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then elif [[ $@ =~ .*pilotpi.arm64 ]]; then # scumaker_pilotpi_arm64 PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest" - elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then - # eagle, excelsior - PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2020-04-01" elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then # posix_rpi_cross, posix_bebop_default PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04" diff --git a/boards/atlflight/cmake_hexagon b/boards/atlflight/cmake_hexagon deleted file mode 160000 index 08fd0a7304..0000000000 --- a/boards/atlflight/cmake_hexagon +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 08fd0a73045346448adf6969660196228b23e1fa diff --git a/boards/atlflight/eagle/CMakeLists.txt b/boards/atlflight/eagle/CMakeLists.txt deleted file mode 100644 index 2055ef2eae..0000000000 --- a/boards/atlflight/eagle/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ diff --git a/boards/atlflight/eagle/cmake/upload.cmake b/boards/atlflight/eagle/cmake/upload.cmake deleted file mode 100644 index c50f88945d..0000000000 --- a/boards/atlflight/eagle/cmake/upload.cmake +++ /dev/null @@ -1,64 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -if("${PX4_PLATFORM}" MATCHES "qurt") - - add_custom_target(upload - COMMAND ${PX4_BOARD_DIR}/scripts/adb_upload.sh - ${PX4_BINARY_DIR}/platforms/qurt/libpx4.so ${PX4_BINARY_DIR}/platforms/qurt/libpx4muorb_skel.so ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/px4.config # source - /usr/share/data/adsp # destination - DEPENDS px4 px4muorb_skel ${PX4_BOARD_DIR}/scripts/adb_upload.sh - COMMENT "uploading px4" - USES_TERMINAL - ) - -else() - - add_custom_target(upload - COMMAND ${PX4_BOARD_DIR}/scripts/adb_upload.sh - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config ${PX4_BINARY_DIR}/etc # source - /home/linaro # destination - DEPENDS px4 ${PX4_BOARD_DIR}/scripts/adb_upload.sh - COMMENT "uploading px4" - USES_TERMINAL - ) - - add_custom_target(sanity - COMMAND ./px4_snapflight_sanitytest.sh -i -t - DEPENDS px4 - WORKING_DIRECTORY ${PX4_BOARD_DIR}/scripts - COMMENT "uploading px4" - USES_TERMINAL - ) - -endif() diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake deleted file mode 100644 index 92f5c80452..0000000000 --- a/boards/atlflight/eagle/default.cmake +++ /dev/null @@ -1,128 +0,0 @@ - -# The Eagle board is the first generation Snapdragon Flight board by Qualcomm. - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon" - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon/toolchain" - ) - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# atlflight toolchain doesn't properly set the compiler, so these aren't set automatically -add_compile_options($<$:-std=gnu99>) -add_compile_options($<$:-std=gnu++11>) - -add_definitions( - -D__PX4_POSIX_EAGLE - -D__PX4_LINUX -) - -px4_add_board( - PLATFORM posix - VENDOR atlflight - MODEL eagle - LABEL default - TOOLCHAIN arm-linux-gnueabihf - ROMFSROOT px4fmu_common - DRIVERS - #barometer # all available barometer drivers - batt_smbus - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #lights/rgbled - #magnetometer # all available magnetometer drivers - pwm_out_sim - qshell/posix - rc_input - smart_battery/batmon - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - #load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - muorb/krait - muorb/test - navigator - rc_update - rover_pos_control - sensors - #sih - temperature_compensation - simulator - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dumpfile - esc_calib - #hardfault_log - led_control - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - #fake_gps - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake deleted file mode 100644 index 5abb30dc82..0000000000 --- a/boards/atlflight/eagle/qurt.cmake +++ /dev/null @@ -1,88 +0,0 @@ - -# The Eagle board is the first generation Snapdragon Flight board by Qualcomm. -# -# This cmake config builds for QURT which is the operating system running on -# the DSP side. - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") - -if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "") - message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set") -else() - set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT}) -endif() - -include(toolchain/Toolchain-qurt) -include(qurt_flags) -include_directories(${HEXAGON_SDK_INCLUDES}) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -# This definition allows to differentiate the specific board. -add_definitions(-D__PX4_QURT_EAGLE) - -px4_add_board( - PLATFORM qurt - VENDOR atlflight - MODEL eagle - LABEL qurt - DRIVERS - barometer/bmp280 - gps - imu/invensense/mpu9250 - #magnetometer/hmc5883 - qshell/qurt - snapdragon_spektrum_rc - MODULES - airspeed_selector - attitude_estimator_q - commander - ekf2 - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - local_position_estimator - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - muorb/adsp - rc_update - rover_pos_control - sensors - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - led_control - mixer - #motor_ramp - motor_test - param - perf - #pwm - #topic_listener - ver - work_queue - ) diff --git a/boards/atlflight/eagle/scripts/.gitignore b/boards/atlflight/eagle/scripts/.gitignore deleted file mode 100644 index 65f4d26f13..0000000000 --- a/boards/atlflight/eagle/scripts/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -minidm.log -px4.log diff --git a/boards/atlflight/eagle/scripts/adb_upload.sh b/boards/atlflight/eagle/scripts/adb_upload.sh deleted file mode 100755 index 5b9475abcc..0000000000 --- a/boards/atlflight/eagle/scripts/adb_upload.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -if [[ "$#" < 2 ]]; then - echo "usage: adb_upload.sh SRC1 [SRC2 ...] DEST" - exit -fi - -# Get last argument -for last; do true; done - -echo "Wait for device..." -adb wait-for-device - -echo "Creating folder structure..." -adb shell mkdir -p $last - -echo "Uploading..." -# Go through source files and push them one by one. -i=0 -for arg -do - if [[ $((i+1)) == "$#" ]]; then - break - fi - # echo "Pushing $arg to $last" - adb push $arg $last - ((i+=1)) -done - -# Make sure they are synced to the file system -echo "Syncing FS..." -adb shell sync diff --git a/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh b/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh deleted file mode 100755 index beed101566..0000000000 --- a/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh +++ /dev/null @@ -1,288 +0,0 @@ -#!/bin/bash - -################################################################################################# -# -# This script loads PX4 binaries to the Snapdragon Flight target and does a quick on-target sanity test. -# -# Pre-requisites: -# - Snapdragon Flight board connected to the host computer via USB cable -# - Snapdragon Flight board must have the latest platform BSP and flight controller addon installed" -# - mini-dm installed on host computer (see https://github.com/ATLFlight/ATLFlightDocs/blob/master/UserGuide.md#adsp) -# - PX4 software was built and binaries are in their usual locations in the Firmware tree. -# -# This script supports two modes: -# - Default mode (supported by PX4 community) -# - Legacy mode (uses proprietary drivers for ESC and RC Receiver, supported by Qualcomm) -# -# For help and cmd line options, run the script with the -h option -# -################################################################################################# - -# Halt on error -set -e - -# Verbose mode -## set -x - -# Mode of operation -readonly MODE_DEFAULT=0 -readonly MODE_LEGACY=1 -readonly MODE_8x96=2 -readonly MODE_MAX=$MODE_8x96 - -readonly RESULT_PASS=0 -readonly RESULT_FAIL=3 -readonly EXIT_ERROR=3 - - -# List of expected strings from the apps proc -declare -a appsproc_strings_present=( - "on udp port 14556 remote port 14550" - ) - -# List of unexpected strings from the apps proc -declare -a appsproc_strings_absent=( - "ERROR" - "Getting Bulk data from fastRPC link" - "Segmentation fault" - ) - -# List of expected strings from the DSP -declare -a dsp_strings_present=( - "EKF aligned" - ) - -# List of unexpected strings from the DSP -declare -a dsp_strings_absent=( - "Segmentation fault" - ) - - -install=0 -test=0 -mode=0 -result=$RESULT_PASS - -# Default mini-dm path (needs to be installed in this location or overriden through cmd line -minidmPath=~/Qualcomm/Hexagon_SDK/3.0/tools/debug/mini-dm/Linux_Debug - -# Default workspace path (parent directory of the script location) -workspace=`pwd`/../../../.. - - -verifypx4test() { - - #TODO: This needs to be fixed. For now, skip string checks for 8x96 platform. - if [ $mode == 2 ]; then - echo -e "[WARNING] Skipping string checks for 8x96 platform" - return - fi - - echo -e "Verifying test results..." - - # verify the presence of expected stings in the apps proc console log - for lineString in "${appsproc_strings_present[@]}" - do - if ! grep -Fq "$lineString" px4.log - then - # code if not found - echo -e "[ERROR] Missing expected string in apps proc log: $lineString" - result=$RESULT_FAIL - fi - done - - # verify the absence of unexpected stings in the apps proc console log - for lineString in "${appsproc_strings_absent[@]}" - do - if grep -Fq "$lineString" px4.log - then - # code if not found - echo -e "[ERROR] Found unexpected string in apps proc log: $lineString" - result=$RESULT_FAIL - fi - done - - echo -e "Displaying the content of the minidm.log" - cat minidm.log - echo -e "Analyzing the log for success and failure indications." - - # verify the presence of expected stings in the DSP console log - for lineString in "${dsp_strings_present[@]}" - do - if ! grep -Fq "$lineString" minidm.log - then - # code if not found - echo -e "[ERROR] Missing expected string in DSP log: $lineString" - result=$RESULT_FAIL - fi - done - - # verify the absence of unexpected stings in the DSP console log - for lineString in "${dsp_strings_absent[@]}" - do - if grep -Fq "$lineString" minidm.log - then - # code if not found - echo -e "[ERROR] Found unexpected string in DSP log: $lineString" - result=$RESULT_FAIL - fi - done - - echo -e "Verification complete." - - if [ $result -eq $RESULT_FAIL ]; then - echo -e "PX4 test result: FAIL" - else - echo -e "PX4 test result: PASS" - fi -} - -installpx4() { - - if [ $install -eq 0 ]; then - echo -e "SKIPPING install" - return 0; - fi - - # Reboot the target before beginning the installation - echo -e "Rebooting the target..." - adb reboot - adb wait-for-usb-device - # Wait a bit longer after bootup, before copying binaries to the target. - sleep 30 - adb devices - - echo -e "Now installing PX4 binaries..." - # Copy binaries to the target - if [ $mode == 0 ]; then - # copy default binaries - echo -e "Copying the PX4 binaries from the eagle_default build tree..." - adb push $workspace/build/atlflight_eagle_qurt/platforms/qurt/libpx4.so /usr/share/data/adsp - adb push $workspace/build/atlflight_eagle_qurt/platforms/qurt/libpx4muorb_skel.so /usr/share/data/adsp - adb push $workspace/build/atlflight_eagle_default/bin/px4 /home/linaro - adb push $workspace/posix-configs/eagle/flight/px4.config /usr/share/data/adsp - adb push $workspace/posix-configs/eagle/flight/mainapp.config /home/linaro - fi - - echo -e "Installation complete." -} - - -testpx4() { - - if [ $test -eq 0 ]; then - echo -e "SKIPPING test" - return 0; - fi - - echo -e "Starting PX4 test..." - - # Remove previous instances of the file - rm px4.log | true - rm minidm.log | true - - # Start mini-dm - echo -e "Starting mini-dm..." - ${minidmPath}/mini-dm > minidm.log & - sleep 5 - # Verify that mini-dm is running - checkProc=$(ps -aef | grep mini-dm | grep -v grep) - - if [ -z "${checkProc}" ]; then - echo "[ERROR] Unable to start mini-dm from path: ${minidmPath}" - exit $EXIT_ERROR - fi - - - # Start PX4 - echo -e "Starting PX4..." - if [ $mode == 2 ]; then - # 8x96 platform - adb shell "/home/root/px4 /home/root/mainapp.config" > px4.log 2>&1 & - else - # 8x74 platform - adb shell "/home/linaro/px4 /home/linaro/mainapp.config" > px4.log 2>&1 & - fi - sleep 20 - # Verify that PX4 is still running - checkProc=$(adb shell "ps -aef | grep px4 | grep -v grep") - if [ -z "${checkProc}" ]; then - echo "[ERROR] PX4 is not running on target!" - exit $EXIT_ERROR - fi - - # Stop the PX4 process on target - adb shell "ps -eaf | grep px4 | grep -v grep | awk '{print $2}' | tr -s ' ' | cut -d' ' -f2 | xargs kill" - sleep 5 - - # Stop the mini-dm - killall mini-dm - - echo -e "PX4 test complete." - - # Verify the results - verifypx4test - - echo -e "For more information, see px4.log and minidm.log." -} - - -usage() { - echo -e "\nThis script can copy PX4 binaries to the Snapdragon Flight target and do a quick on-target sanity test.\n" - echo -e "Pre-requisites:" - echo -e "- Snapdragon Flight board must be connected to host computer via USB" - echo -e "- Snapdragon Flight board must have the latest platform BSP and flight controller addon installed" - echo -e "- mini-dm must be installed on host computer (see https://github.com/ATLFlight/ATLFlightDocs/blob/master/UserGuide.md#adsp)" - echo -e "- PX4 software was built and binaries are in their usual locations in the tree\n" - echo -e "USAGE:\n ${0} [-m mode] [-i] [-t] [-l ]" - echo -e " -m --> Build mode (0 = default mode, 1 = legacy mode)" - echo -e " -i --> Install the PX4 binaries" - echo -e " -t --> Test PX4 on target" - echo -e " -l --> location of the mini-dm executable (Default: ${minidmPath})" - echo -e " -h --> Display this help information" -} - -# Parse the command line options -while getopts "m:l:ith" opt; - do - case $opt in - m) - if [ $OPTARG -gt $MODE_MAX ]; then - echo "Invalid mode: $OPTARG (max allowed is $MODE_MAX)" - exit $EXIT_ERROR - fi - mode=$OPTARG - echo "Will run the script in mode $mode." - ;; - i) - install=1 - ;; - t) - test=1 - ;; - l) - minidmPath=$OPTARG - ;; - h) - usage - exit 0 - ;; - :) - echo "Option -$OPTARG requires an argument" >&2 - exit 1;; - ?) - echo "Unknown arg $opt" - usage - exit 1 - ;; - esac -done - -# Install the PX4 binaries -installpx4 - -# Run the sanity test -testpx4 - -exit $result - diff --git a/boards/atlflight/eagle/src/CMakeLists.txt b/boards/atlflight/eagle/src/CMakeLists.txt deleted file mode 100644 index e2fb760a04..0000000000 --- a/boards/atlflight/eagle/src/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -add_library(drivers_board - board_config.h - i2c.cpp - init.c - spi.cpp -) diff --git a/boards/atlflight/eagle/src/board_config.h b/boards/atlflight/eagle/src/board_config.h deleted file mode 100644 index b54dc0ae54..0000000000 --- a/boards/atlflight/eagle/src/board_config.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file board_config.h - * - * EAGLE internal definitions - */ - -#pragma once - -#define BOARD_OVERRIDE_UUID "EAGLEID000000000" // must be of length 16 -#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_EAGLE - -/* - * I2C busses - */ -#define PX4_NUMBER_I2C_BUSES 3 - -// Battery ADC channels - -#include -#include diff --git a/boards/atlflight/eagle/src/i2c.cpp b/boards/atlflight/eagle/src/i2c.cpp deleted file mode 100644 index 694c541883..0000000000 --- a/boards/atlflight/eagle/src/i2c.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { - initI2CBusExternal(2), - initI2CBusExternal(3), - initI2CBusExternal(9), -}; diff --git a/boards/atlflight/eagle/src/init.c b/boards/atlflight/eagle/src/init.c deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/boards/atlflight/eagle/src/spi.cpp b/boards/atlflight/eagle/src/spi.cpp deleted file mode 100644 index e740286c8a..0000000000 --- a/boards/atlflight/eagle/src/spi.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(1, { - initSPIDevice(DRV_IMU_DEVTYPE_MPU9250), - }), -}; diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake deleted file mode 100644 index 530b983f9f..0000000000 --- a/boards/atlflight/excelsior/default.cmake +++ /dev/null @@ -1,126 +0,0 @@ - -# Excelsior is the code name of a board currently in development. - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon" - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon/toolchain" - ) - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# atlflight toolchain doesn't properly set the compiler, so these aren't set automatically -add_compile_options($<$:-std=gnu99>) -add_compile_options($<$:-std=gnu++11>) - -add_definitions( - -D__PX4_POSIX_EXCELSIOR - -D__PX4_LINUX -) - -px4_add_board( - PLATFORM posix - VENDOR atlflight - MODEL excelsior - LABEL default - TOOLCHAIN arm-oemllib32-linux-gnueabi - DRIVERS - #barometer # all available barometer drivers - batt_smbus - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #lights/rgbled - #magnetometer # all available magnetometer drivers - pwm_out_sim - qshell/posix - rc_input - smart_battery/batmon - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - #load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - muorb/krait - muorb/test - navigator - rc_update - rover_pos_control - sensors - #sih - simulator - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dumpfile - esc_calib - #hardfault_log - led_control - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - #fake_gps - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake deleted file mode 100644 index b45f0af258..0000000000 --- a/boards/atlflight/excelsior/qurt.cmake +++ /dev/null @@ -1,88 +0,0 @@ - -# Excelsior is the code name of a board currently in development. -# -# This cmake config builds for QURT which is the operating system running on -# the DSP side. - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") - -if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "") - message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set") -else() - set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT}) -endif() - -include(toolchain/Toolchain-qurt) -include(qurt_flags) -include_directories(${HEXAGON_SDK_INCLUDES}) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -# This definition allows to differentiate the specific board. -add_definitions(-D__PX4_QURT_EXCELSIOR) - -px4_add_board( - PLATFORM qurt - VENDOR atlflight - MODEL excelsior - LABEL qurt - DRIVERS - barometer/bmp280 - gps - imu/invensense/mpu9250 - magnetometer/hmc5883 - qshell/qurt - snapdragon_spektrum_rc - MODULES - airspeed_selector - attitude_estimator_q - commander - ekf2 - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - local_position_estimator - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - muorb/adsp - rc_update - rover_pos_control - sensors - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - led_control - mixer - #motor_ramp - motor_test - param - perf - #pwm - #topic_listener - ver - work_queue - ) diff --git a/boards/atlflight/excelsior/src/CMakeLists.txt b/boards/atlflight/excelsior/src/CMakeLists.txt deleted file mode 100644 index df3582f15a..0000000000 --- a/boards/atlflight/excelsior/src/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -add_library(drivers_board - board_config.h - init.c - spi.cpp - i2c.cpp -) diff --git a/boards/atlflight/excelsior/src/board_config.h b/boards/atlflight/excelsior/src/board_config.h deleted file mode 100644 index bf9426f3c7..0000000000 --- a/boards/atlflight/excelsior/src/board_config.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file board_config.h - * - * EXCELSIOR internal definitions - */ - -#pragma once - -#define BOARD_OVERRIDE_UUID "EAGLEID000000000" // must be of length 16 -#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_EAGLE - -/* - * I2C busses - */ -#define PX4_NUMBER_I2C_BUSES 3 - -#include -#include diff --git a/boards/atlflight/excelsior/src/i2c.cpp b/boards/atlflight/excelsior/src/i2c.cpp deleted file mode 100644 index 694c541883..0000000000 --- a/boards/atlflight/excelsior/src/i2c.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { - initI2CBusExternal(2), - initI2CBusExternal(3), - initI2CBusExternal(9), -}; diff --git a/boards/atlflight/excelsior/src/init.c b/boards/atlflight/excelsior/src/init.c deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/boards/atlflight/excelsior/src/spi.cpp b/boards/atlflight/excelsior/src/spi.cpp deleted file mode 100644 index e740286c8a..0000000000 --- a/boards/atlflight/excelsior/src/spi.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(1, { - initSPIDevice(DRV_IMU_DEVTYPE_MPU9250), - }), -}; diff --git a/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp b/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp index 92c9fcf753..04d31dd44d 100644 --- a/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp +++ b/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp @@ -244,22 +244,6 @@ RingBuffer::force(double val) return force(&val, sizeof(val)); } -// FIXME - clang crashes on this get() call -#ifdef __PX4_QURT -#define __PX4_SBCAP my_sync_bool_compare_and_swap -static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) -{ - if (*a == b) { - *a = c; - return true; - } - - return false; -} - -#else -#define __PX4_SBCAP __sync_bool_compare_and_swap -#endif bool RingBuffer::get(void *val, size_t val_size) { @@ -284,7 +268,7 @@ RingBuffer::get(void *val, size_t val_size) } /* if the tail pointer didn't change, we got our item */ - } while (!__PX4_SBCAP(&_tail, candidate, next)); + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); return true; diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index fb2944b179..660b2e622d 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -179,9 +179,7 @@ function(px4_add_module) endif() set_target_properties(${MODULE} PROPERTIES STACK_MAX ${STACK_MAX}) - if(${PX4_PLATFORM} STREQUAL "qurt") - set_property(TARGET ${MODULE} PROPERTY POSITION_INDEPENDENT_CODE TRUE) - elseif(${PX4_PLATFORM} STREQUAL "nuttx") + if(${PX4_PLATFORM} STREQUAL "nuttx") target_compile_options(${MODULE} PRIVATE -Wframe-larger-than=${STACK_MAX}) endif() diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index c9803450c3..b103e5b6ef 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -33,7 +33,7 @@ set(SRCS) -if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader") +if(NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader") list(APPEND SRCS px4_log.cpp ) diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index d99a197817..b2d828efdb 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -84,10 +84,6 @@ public: */ inline T load() const { -#ifdef __PX4_QURT - return _value; -#else - #if defined(__PX4_NUTTX) if (!__atomic_always_lock_free(sizeof(T), 0)) { @@ -101,8 +97,6 @@ public: { return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); } - -#endif } /** @@ -110,10 +104,6 @@ public: */ inline void store(T value) { -#ifdef __PX4_QURT - _value = value; -#else - #if defined(__PX4_NUTTX) if (!__atomic_always_lock_free(sizeof(T), 0)) { @@ -126,8 +116,6 @@ public: { __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); } - -#endif } /** @@ -136,11 +124,6 @@ public: */ inline T fetch_add(T num) { -#ifdef __PX4_QURT - // TODO: fix - return _value++; -#else - #if defined(__PX4_NUTTX) if (!__atomic_always_lock_free(sizeof(T), 0)) { @@ -154,8 +137,6 @@ public: { return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); } - -#endif } /** @@ -301,13 +282,7 @@ public: } private: -#ifdef __PX4_QURT - // It seems that __atomic_store and __atomic_load are not supported on Qurt, - // so the best that we can do is to use volatile. - volatile T _value{}; -#else T _value {}; -#endif }; using atomic_int = atomic; diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 7038130b2f..def1786a3a 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -89,33 +89,14 @@ constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } #define USEC_PER_TICK (1000000/PX4_TICKS_PER_SEC) #define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) -#ifdef __PX4_QURT - -// QURT specific -# include "dspal_math.h" -# define PX4_ROOTFSDIR "." -# define PX4_TICKS_PER_SEC 1000L - -#else // __PX4_QURT - -// All POSIX except QURT. - __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -# if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) -# define PX4_ROOTFSDIR "/home/linaro" -# else -# define PX4_ROOTFSDIR "." -# endif - -#endif // __PX4_QURT +#define PX4_ROOTFSDIR "." #define PX4_STORAGEDIR PX4_ROOTFSDIR -#endif // __PX4_POSIX -#if defined(__PX4_POSIX) /**************************************************************************** * Defines for POSIX and ROS ****************************************************************************/ diff --git a/platforms/common/include/px4_platform_common/shmem.h b/platforms/common/include/px4_platform_common/shmem.h deleted file mode 100644 index 7d7d184988..0000000000 --- a/platforms/common/include/px4_platform_common/shmem.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Ramakrishna Kintada. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -#define MAX_SHMEM_PARAMS 2000 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info)) - -#define PARAM_BUFFER_SIZE (MAX_SHMEM_PARAMS / 8 + 1) - -struct shmem_info { - union param_value_u params_val[MAX_SHMEM_PARAMS]; - unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by krait*/ - unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by adsp*/ - -#ifdef __PX4_NUTTX -}; -#else -} __attribute__((packed)); -#endif - -#if (defined(__PX4_POSIX_EXCELSIOR) || defined(__PX4_QURT_EXCELSIOR)) -#define MAP_ADDRESS 0x861FC000 -#else -#define MAP_ADDRESS 0xfbfc000 -#endif -#define MAP_SIZE 16384 -#define MAP_MASK (MAP_SIZE - 1) - -#define LOCK_OFFSET 0 -#define LOCK_SIZE 4 -#define LOCK_MEM 1 -#define UNLOCK_MEM 2 - -#define TYPE_MASK 0x1 - -extern bool handle_in_range(param_t); - -#ifdef __PX4_QURT -extern struct shmem_info *shmem_info_p; - -int get_shmem_lock(const char *caller_file_name, int caller_line_number); -void release_shmem_lock(const char *caller_file_name, int caller_line_number); -void init_shared_memory(void); - -void copy_params_to_shmem(const param_info_s *param_info_base); -#endif - -void update_to_shmem(param_t param, union param_value_u value); -int update_from_shmem(param_t param, union param_value_u *value); -void update_index_from_shmem(void); diff --git a/platforms/common/include/px4_platform_common/workqueue.h b/platforms/common/include/px4_platform_common/workqueue.h index b4fa7ad1a3..8b7ee24163 100644 --- a/platforms/common/include/px4_platform_common/workqueue.h +++ b/platforms/common/include/px4_platform_common/workqueue.h @@ -45,10 +45,6 @@ #include #include -#ifdef __PX4_QURT -#include -#endif - __BEGIN_DECLS #define HPWORK 0 diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index b6a172e7b8..48b88c7290 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -49,7 +49,7 @@ WorkQueue::WorkQueue(const wq_config_t &config) : // set the threads name #ifdef __PX4_DARWIN pthread_setname_np(_config.name); -#elif !defined(__PX4_QURT) +#else pthread_setname_np(pthread_self(), _config.name); #endif diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 927c3e35b2..f9ed73af56 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -25,73 +25,46 @@ px4_posix_generate_alias( PREFIX ${PX4_SHELL_COMMAND_PREFIX} ) -if (("${PX4_BOARD}" MATCHES "atlflight_eagle") OR ("${PX4_BOARD}" MATCHES "atlflight_excelsior")) - include(fastrpc) - include(linux_app) - - FASTRPC_STUB_GEN(../qurt/px4muorb.idl) - - LINUX_APP( - APP_NAME px4 - IDL_NAME px4muorb - APPS_DEST "/home/linaro" - SOURCES - px4muorb_stub.c - src/px4/common/main.cpp - apps.cpp - LINK_LIBS - -Wl,--start-group - px4_layer - parameters - ${module_libraries} - ${FASTRPC_ARM_LIBS} - pthread m rt - -Wl,--end-group - ) - -else() - add_executable(px4 - src/px4/common/main.cpp - apps.cpp - ) - - target_link_libraries(px4 - PRIVATE - ${module_libraries} - pthread m - - # horrible circular dependencies that need to be teased apart - px4_layer px4_platform - work_queue - parameters +add_executable(px4 + src/px4/common/main.cpp + apps.cpp ) - if(NOT APPLE) - target_link_libraries(px4 PRIVATE rt) - endif() +target_link_libraries(px4 + PRIVATE + ${module_libraries} + pthread m - target_link_libraries(px4 PRIVATE uORB) - - #============================================================================= - # install - # - - # TODO: extend to snapdragon - - # px4 dirs - install( - DIRECTORY - ${PROJECT_SOURCE_DIR}/posix-configs - ${PROJECT_SOURCE_DIR}/test - ${CMAKE_BINARY_DIR}/etc - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} - DESTINATION - ${PROJECT_NAME} - USE_SOURCE_PERMISSIONS - ) + # horrible circular dependencies that need to be teased apart + px4_layer px4_platform + work_queue + parameters +) +if(NOT APPLE) + target_link_libraries(px4 PRIVATE rt) endif() +target_link_libraries(px4 PRIVATE uORB) + +#============================================================================= +# install +# + +# TODO: extend to snapdragon + +# px4 dirs +install( + DIRECTORY + ${PROJECT_SOURCE_DIR}/posix-configs + ${PROJECT_SOURCE_DIR}/test + ${CMAKE_BINARY_DIR}/etc + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} + DESTINATION + ${PROJECT_NAME} + USE_SOURCE_PERMISSIONS +) + # Module Symlinks px4_posix_generate_symlinks( MODULE_LIST ${module_libraries} diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 48656a2420..34b65cdf44 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -36,17 +36,6 @@ add_subdirectory(px4_daemon) add_subdirectory(lockstep_scheduler) set(EXTRA_DEPENDS) -if("${CONFIG_SHMEM}" STREQUAL "1") - list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") - include(hexagon_sdk) - include_directories(${HEXAGON_SDK_INCLUDES}) - include_directories(${PX4_BINARY_DIR}/platforms/posix) - list(APPEND SHMEM_SRCS - shmem_posix.cpp - ) - add_definitions(-DCONFIG_SHMEM=1) - set(EXTRA_DEPENDS generate_px4muorb_stubs) -endif() add_library(px4_layer px4_posix_impl.cpp @@ -57,7 +46,6 @@ add_library(px4_layer drv_hrt.cpp cpuload.cpp print_load.cpp - ${SHMEM_SRCS} ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index 50b5ad5dfd..901ffdd722 100644 --- a/platforms/posix/src/px4/common/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -317,15 +317,6 @@ int main(int argc, char **argv) pxh.run_pxh(); } - // When we exit, we need to stop muorb on Snapdragon. - -#ifdef __PX4_POSIX_EAGLE - // Sending muorb stop is needed if it is running to exit cleanly. - // TODO: we should check with px4_task_is_running("muorb") before stopping it. - std::string muorb_stop_cmd("muorb stop"); - px4_daemon::Pxh::process_line(muorb_stop_cmd, true); -#endif - std::string cmd("shutdown"); px4_daemon::Pxh::process_line(cmd, true); @@ -451,10 +442,7 @@ void sig_int_handler(int sig_num) void set_cpu_scaling() { -#ifdef __PX4_POSIX_EAGLE - // On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run - // at the maximum frequency all the time. - // Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon. +#if 0 system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor"); system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor"); diff --git a/platforms/posix/src/px4/common/px4_daemon/server_io.cpp b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp index b0c73fcb5b..d06aa1922f 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server_io.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp @@ -73,19 +73,6 @@ FILE *get_stdout(bool *isatty_) return stdout; } -#ifdef __PX4_POSIX_EAGLE - - // XXX FIXME: thread_data_ptr is set to 0x1 in the main thread on Snapdragon - // even though the pthread_key has been created. - // We can catch this using the check below but we have no clue why this happens. - if (thread_data_ptr == (void *)0x1) { - if (isatty_) { *isatty_ = isatty(1); } - - return stdout; - } - -#endif - if (thread_data_ptr->thread_stdout == nullptr) { if (isatty_) { *isatty_ = isatty(1); } diff --git a/platforms/posix/src/px4/common/shmem_posix.cpp b/platforms/posix/src/px4/common/shmem_posix.cpp deleted file mode 100644 index 8f97d3f2b4..0000000000 --- a/platforms/posix/src/px4/common/shmem_posix.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include "px4muorb.h" - -//#define SHMEM_DEBUG - -static uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; -extern unsigned char *adsp_changed_index; - -struct param_wbuf_s { - union param_value_u val; - param_t param; - bool unsaved; -}; - -/*update value and param's change bit in shared memory*/ -void update_to_shmem(param_t param, union param_value_u value) -{ - if (px4muorb_param_update_to_shmem(param, (unsigned char *) &value, sizeof(value))) { - PX4_ERR("krait update param %u failed", param); - } -} - -void update_index_from_shmem(void) -{ - if (!adsp_changed_index) { - PX4_ERR("%s no param buffer", __FUNCTION__); - return; - } - - px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE); -} - -static void update_value_from_shmem(param_t param, union param_value_u *value) -{ - if (px4muorb_param_update_value_from_shmem(param, (unsigned char *) value, sizeof(union param_value_u))) { - PX4_ERR("%s get param failed", __FUNCTION__); - } -} - -int update_from_shmem(param_t param, union param_value_u *value) -{ - unsigned int byte_changed, bit_changed; - unsigned int retval = 0; - - if (!adsp_changed_index) { - PX4_ERR("%s no param buffer", __FUNCTION__); - return 0; - } - - update_from_shmem_current_time = hrt_absolute_time(); - - if ((update_from_shmem_current_time - update_from_shmem_prev_time) - > 1000000) { //update every 1 second - update_from_shmem_prev_time = update_from_shmem_current_time; - update_index_from_shmem(); - } - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - - if (adsp_changed_index[byte_changed] & bit_changed) { - update_value_from_shmem(param, value); - adsp_changed_index[byte_changed] &= ~bit_changed; //clear the bit - retval = 1; - } - - //else {PX4_INFO("no change to param %s", param_name(param));} - - PX4_DEBUG("%s %d bit on adsp index[%d]", - (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); - - return retval; -} diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt deleted file mode 100644 index a7bf297126..0000000000 --- a/platforms/qurt/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ - -px4_add_git_submodule(TARGET git_dspal PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal") - -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal/cmake_hexagon") -include(toolchain/Toolchain-qurt) -include(fastrpc) -include(qurt_lib) -include(qurt_flags) - -include_directories(${CMAKE_CURRENT_BINARY_DIR}) - -get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) - -px4_qurt_generate_builtin_commands( - OUT ${PX4_BINARY_DIR}/apps - MODULE_LIST ${module_libraries} -) - -FASTRPC_STUB_GEN(px4muorb.idl) - -add_definitions(-D__QAIC_SKEL_EXPORT=__EXPORT) - -# Enable build without HexagonSDK to check link dependencies -if ("${QURT_ENABLE_STUBS}" STREQUAL "1") - - add_definitions(-D QURT_EXE_BUILD=1) - - include_directories( - ${CMAKE_CURRENT_BINARY_DIR} - ${FASTRPC_DSP_INCLUDES} - ) - - add_executable(px4 - ${PX4_BINARY_DIR}/apps.cpp - ${PX4_BINARY_DIR}/platforms/qurt/px4muorb_skel.c - ) - - target_link_libraries(px4 PRIVATE ${module_libraries}) - -else() - # Generate the DSP lib and stubs but not the apps side executable - # The Apps side executable is generated via the posix_eagle_xxxx target - QURT_LIB(LIB_NAME px4 - IDL_NAME px4muorb - SOURCES - ${PX4_BINARY_DIR}/apps.cpp - LINK_LIBS - modules__muorb__adsp - ${module_libraries} - m - ) - -endif() - -# board defined upload helper -if(EXISTS "${PX4_BOARD_DIR}/cmake/upload.cmake") - include(${PX4_BOARD_DIR}/cmake/upload.cmake) -endif() diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake deleted file mode 100644 index 054ab22c65..0000000000 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ /dev/null @@ -1,188 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -#============================================================================= -# -# Defined functions in this file -# -# OS Specific Functions -# -# * px4_qurt_add_firmware -# * px4_qurt_generate_builtin_commands -# * px4_qurt_add_export -# * px4_qurt_generate_romfs -# -# Required OS Inteface Functions -# -# * px4_os_add_flags -# * px4_os_determine_build_chip -# * px4_os_prebuild_targets -# - -#============================================================================= -# -# px4_qurt_generate_builtin_commands -# -# This function generates the builtin_commands.c src for qurt -# -# Usage: -# px4_qurt_generate_builtin_commands( -# MODULE_LIST -# OUT ) -# -# Input: -# MODULE_LIST : list of modules -# -# Output: -# OUT : stem of generated apps.cpp/apps.h ("apps"). -# -# Example: -# px4_qurt_generate_builtin_commands( -# OUT MODULE_LIST px4_simple_app) -# -function(px4_qurt_generate_builtin_commands) - px4_parse_function_args( - NAME px4_qurt_generate_builtin_commands - ONE_VALUE OUT - MULTI_VALUE MODULE_LIST - REQUIRED MODULE_LIST OUT - ARGN ${ARGN}) - - set(builtin_apps_string) - set(builtin_apps_decl_string) - set(command_count 0) - foreach(module ${MODULE_LIST}) - foreach(property MAIN STACK_MAIN PRIORITY) - get_target_property(${property} ${module} ${property}) - endforeach() - if (MAIN) - set(builtin_apps_string - "${builtin_apps_string}\tapps[\"${MAIN}\"] = ${MAIN}_main;\n") - set(builtin_apps_decl_string - "${builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n") - math(EXPR command_count "${command_count}+1") - endif() - endforeach() - configure_file(${PX4_SOURCE_DIR}/platforms/common/apps.cpp.in ${OUT}.cpp) - configure_file(${PX4_SOURCE_DIR}/platforms/common/apps.h.in ${OUT}.h) -endfunction() - -#============================================================================= -# -# px4_os_add_flags -# -# Set the qurt build flags. -# -# Usage: -# px4_os_add_flags() -# -function(px4_os_add_flags) - - set(DSPAL_ROOT platforms/qurt/dspal) - include_directories( - ${DSPAL_ROOT}/include - ${DSPAL_ROOT}/sys - ${DSPAL_ROOT}/sys/sys - - platforms/posix/include - platforms/qurt/include - ) - - add_definitions( - -D__PX4_POSIX - -D__PX4_QURT - ) - - add_compile_options( - -fPIC - -fmath-errno - - -Wno-unknown-warning-option - -Wno-cast-align - ) - - # Clear -rdynamic flag which fails for hexagon - set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS) - set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS) - -endfunction() - -#============================================================================= -# -# px4_os_determine_build_chip -# -# Sets PX4_CHIP and PX4_CHIP_MANUFACTURER. -# -# Usage: -# px4_os_determine_build_chip() -# -function(px4_os_determine_build_chip) - - # always use generic chip and chip manufacturer - set(PX4_CHIP "generic" CACHE STRING "PX4 Chip" FORCE) - set(PX4_CHIP_MANUFACTURER "generic" CACHE STRING "PX4 Chip Manufacturer" FORCE) - -endfunction() - -#============================================================================= -# -# px4_os_prebuild_targets -# -# This function generates os dependent targets -# -# Usage: -# px4_os_prebuild_targets( -# OUT -# BOARD -# ) -# -# Input: -# BOARD : board -# -# Output: -# OUT : the target list -# -# Example: -# px4_os_prebuild_targets(OUT target_list BOARD px4_fmu-v2) -# -function(px4_os_prebuild_targets) - px4_parse_function_args( - NAME px4_os_prebuild_targets - ONE_VALUE OUT BOARD - REQUIRED OUT - ARGN ${ARGN}) - - add_library(prebuild_targets INTERFACE) - add_dependencies(prebuild_targets DEPENDS uorb_headers) - -endfunction() diff --git a/platforms/qurt/dspal b/platforms/qurt/dspal deleted file mode 160000 index 0322a4e345..0000000000 --- a/platforms/qurt/dspal +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0322a4e345e48ea28cb1cee14a33033cdaf0b16a diff --git a/platforms/qurt/include/crc32.h b/platforms/qurt/include/crc32.h deleted file mode 100644 index 34e080b1c2..0000000000 --- a/platforms/qurt/include/crc32.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * include/crc.h - * - * Copyright (C) 2010 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef __INCLUDE_CRC32_H -#define __INCLUDE_CRC32_H - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -#ifdef __cplusplus -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -/**************************************************************************** - * Name: crc32part - * - * Description: - * Continue CRC calculation on a part of the buffer. - * - ****************************************************************************/ - -EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); - -/**************************************************************************** - * Name: crc32 - * - * Description: - * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' - * - ****************************************************************************/ - -EXTERN uint32_t crc32(const uint8_t *src, size_t len); - -#undef EXTERN -#ifdef __cplusplus -} -#endif - -#endif /* __INCLUDE_CRC32_H */ diff --git a/platforms/qurt/include/hrt_work.h b/platforms/qurt/include/hrt_work.h deleted file mode 100644 index 36b2446c14..0000000000 --- a/platforms/qurt/include/hrt_work.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include - -#pragma once - -__BEGIN_DECLS - -extern sem_t _hrt_work_lock; -extern struct wqueue_s g_hrt_work; - -void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); -void hrt_work_cancel(struct work_s *work); - -static inline void hrt_work_lock(void); -static inline void hrt_work_unlock(void); - -static inline void hrt_work_lock() -{ - //PX4_INFO("hrt_work_lock"); - sem_wait(&_hrt_work_lock); -} - -static inline void hrt_work_unlock() -{ - //PX4_INFO("hrt_work_unlock"); - sem_post(&_hrt_work_lock); -} - -__END_DECLS - diff --git a/platforms/qurt/include/queue.h b/platforms/qurt/include/queue.h deleted file mode 100644 index d6315ca172..0000000000 --- a/platforms/qurt/include/queue.h +++ /dev/null @@ -1,134 +0,0 @@ -/************************************************************************ - * include/queue.h - * - * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ************************************************************************/ - -#ifndef __INCLUDE_QUEUE_H -#define __INCLUDE_QUEUE_H - -/************************************************************************ - * Included Files - ************************************************************************/ - -#include - -#ifdef __cplusplus -#include // NULL -#else -#include // NULL -#endif - -/************************************************************************ - * Pre-processor Definitions - ************************************************************************/ - -#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) -#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) - -#define sq_next(p) ((p)->flink) -#define dq_next(p) ((p)->flink) -#define dq_prev(p) ((p)->blink) - -#define sq_empty(q) ((q)->head == NULL) -#define dq_empty(q) ((q)->head == NULL) - -#define sq_peek(q) ((q)->head) -#define dq_peek(q) ((q)->head) - -// Required for Linux -#define FAR - -/************************************************************************ - * Global Type Declarations - ************************************************************************/ - -struct sq_entry_s { - FAR struct sq_entry_s *flink; -}; -typedef struct sq_entry_s sq_entry_t; - -struct dq_entry_s { - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; -}; -typedef struct dq_entry_s dq_entry_t; - -struct sq_queue_s { - FAR sq_entry_t *head; - FAR sq_entry_t *tail; -}; -typedef struct sq_queue_s sq_queue_t; - -struct dq_queue_s { - FAR dq_entry_t *head; - FAR dq_entry_t *tail; -}; -typedef struct dq_queue_s dq_queue_t; - -/************************************************************************ - * Global Function Prototypes - ************************************************************************/ - -#ifdef __cplusplus -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); -EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); -EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); -EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); -EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); - -EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); -EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); -EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); -EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); -EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); - -#undef EXTERN -#ifdef __cplusplus -} -#endif - -#endif /* __INCLUDE_QUEUE_H_ */ - diff --git a/platforms/qurt/include/qurt_log.h b/platforms/qurt/include/qurt_log.h deleted file mode 100644 index ad32e8999a..0000000000 --- a/platforms/qurt/include/qurt_log.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -__BEGIN_DECLS - -__EXPORT extern uint64_t hrt_absolute_time(void); - -//void qurt_log(int level, const char *file, int line, const char *format, ...); - -// declaration to make the compiler happy. This symbol is part of the adsp static image. -void HAP_debug(const char *msg, int level, const char *filename, int line); - -#ifndef qurt_log_defined -#define qurt_log_defined -static __inline void qurt_log(int level, const char *file, int line, - const char *format, ...) -{ - char buf[256]; - va_list args; - va_start(args, format); - vsnprintf(buf, sizeof(buf), format, args); - va_end(args); - HAP_debug(buf, level, file, line); -} -#endif - -__END_DECLS diff --git a/platforms/qurt/px4muorb.idl b/platforms/qurt/px4muorb.idl deleted file mode 100644 index c8872c09e3..0000000000 --- a/platforms/qurt/px4muorb.idl +++ /dev/null @@ -1,184 +0,0 @@ -//======================================================================= -// Copyright (c) 2016, Mark Charlebois. All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are -// met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of The Linux Foundation nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED -// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS -// BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE -// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN -// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//========================================================================= - -#include "AEEStdDef.idl" - -interface px4muorb{ - - /** - * interface method to start the uorb service and initialize the muorb - */ - AEEResult orb_initialize(); - - /** - * Interface to set an offset to hrt_absolute_time on the DSP. - * - * @param time_diff_us: time difference of the DSP clock to Linux clock. - * A positive number means the Linux clock is ahead of the DSP one. - */ - AEEResult set_absolute_time_offset( in long time_diff_us ); - - /** - * Interface to request hrt_absolute_time on the DSP. - * @param time_us: pointer to time in us - */ - AEEResult get_absolute_time(rout unsigned long long time_us); - - /** - * Interface to update param for krait. - * - * @param param: param index. - * @param value: param value. - */ - AEEResult param_update_to_shmem( in unsigned long param, in sequence value); - - /** - * Interface to update index for krait. - * @param data: param index array. - */ - AEEResult param_update_index_from_shmem( rout sequence data); - - /** - * Interface to get param value for krait. - * - * @param param: param index. - * @param value: param value. - */ - AEEResult param_update_value_from_shmem( in unsigned long param, rout sequence value); - - /** - * Interface called from krait to inform of a published topic. - * @param topic_name: name of the topic being advertised - */ - AEEResult topic_advertised(in string topic_name); - - /** - * Interface called from krait to inform of a published topic. - * @param topic_name: name of the topic being advertised - */ - AEEResult topic_unadvertised(in string topic_name); - - /** - * Interface to add a subscriber to the identified topic - * @param topic_name - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult add_subscriber( in string topic_name ); - - /** - * Interface to remove a subscriber for the identified topic. - * @param topic_name - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult remove_subscriber( in string topic_name ); - - /** - * Interface called from krait for topic data. - * @param topic_name - * @param data - * a sequence of bytes for the passed data stream. - * as per the HExagon Documention, the max size of this stream is 255 bytes. - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult send_topic_data( in string topic_name, in sequence data ); - - /** - * Inteface to check if there are subscribers on the remote adsp client - * This inteface is required as the krait app can be restarted without adsp - * being re-started. In this scenario the krait app does not know if there - * is subscriber on the remote end(ie adsp). - * @param topic_name - * The name of the topic for which the subscription needs to be checked. - * @param rout int status - * The status of the subscription, 0=no-subscribers, 1 = more than one subscriber. - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult is_subscriber_present( in string topic_name, rout long status ); - - /** - * Interface to receive data from adsp. Since there is only one interface, the different message - * types are identified by the msg_type field. - * @param msg_type - * The possible values are: - * 1 ==> add_subscriber - * 2 ==> remove_subscriber - * 3 ==> recieve_topic_data - * 4 ==> advertised_topic - * @note: for message types, 1 & 2, the data pointer returned is null with length of 0. - * @param topic_name - * The topic being returned. - * @param data - * the data stream - * @param bytes_returned - * The number of bytes returned in the byte buffer. - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult receive_msg( rout long msg_type, rout string topic_name, rout sequence data, rout long bytes_returned ); - - /** - * Since the receive_msg is a blocking call, the client will not be able to peform a clean shutdown. Calling this - * function will unblock the receive msg an return an error code. - * Ideally this should be implemented as a timeout for the #receive_msg call. - * @param none - * @return status - * 0 = success - * all others is a failure. - **/ - AEEResult unblock_recieve_msg(); - - /** - * This interface will perform a bulk read from the adsp and return the data buffer. - * The structure of the messages is as follows - * +----------------+-----------+---------------+----------+----------------+ - * | Topic Name Len | datal_len |TopicName(nullterminated) | data bytes | - * +----------------+-----------+---------------+----------+----------------+ - * |<-- 2 bytes -->|<-2bytes-> |<-- topicnamelen bytes -->|<-datalenbytes->| - * +----------------+---------------------------+----------+----------------+ - * @param data - * The output buffer where the data needs to be copied - * @param bytes_returned - * The number of bytes the buffer is filled up by. - * @param topic_count - * The numbe of topics filled in the buffer. - * @return status - * 0 = success - * all others is a failure. - **/ - AEEResult receive_bulk_data( rout sequence data, rout long bytes_returned, rout long topic_count ); -}; diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt deleted file mode 100644 index 929788b587..0000000000 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -add_subdirectory(common) - -add_subdirectory(${PX4_CHIP_MANUFACTURER}) - diff --git a/platforms/qurt/src/px4/common/CMakeLists.txt b/platforms/qurt/src/px4/common/CMakeLists.txt deleted file mode 100644 index c64e4765d1..0000000000 --- a/platforms/qurt/src/px4/common/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -include(hexagon_sdk) - -include_directories(${HEXAGON_8074_INCLUDES}) - -set(QURT_LAYER_SRCS - px4_qurt_impl.cpp - tasks.cpp - lib_crc32.c - drv_hrt.cpp - qurt_stubs.c - main.cpp - shmem_qurt.cpp - px4_init.cpp - ) - -if ("${QURT_ENABLE_STUBS}" STREQUAL "1") - list(APPEND QURT_LAYER_SRCS - stubs/posix.c - stubs/qurt.c - ) -endif() - -add_library(px4_layer - ${QURT_LAYER_SRCS} - ${CONFIG_SRC} -) -target_link_libraries(px4_layer PRIVATE work_queue) -add_dependencies(px4_layer uorb_msgs) # dataman requires mission diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c deleted file mode 100644 index 6519fcce32..0000000000 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file commands_muorb_test.c - * Commands to run for the "qurt_muorb_test" config - * - * @author Mark Charlebois - */ - -const char *get_commands(void); -const char *get_commands() -{ - - static const char *commands = - "param set CAL_GYRO0_ID 2293760\n" - "param set CAL_ACC0_ID 1310720\n" - "param set CAL_ACC1_ID 1376256\n" - "param set CAL_MAG0_ID 196608\n" -// "rgbled start\n" -// "tone_alarm start\n" - "rc_update start\n" - "commander start --hil\n" - "sensors start\n" - "ekf2 start\n" - "mc_hover_thrust_estimator start\n" - "flight_mode_manager start\n" - "mc_pos_control start\n" - "mc_att_control start\n" - "mc_rate_control start\n" - "sleep 1\n" - "pwm_out_sim start\n" - "param set RC1_MAX 2015\n" - "param set RC1_MIN 996\n" - "param set RC1_TRIM 1502\n" - "param set RC1_REV -1\n" - "param set RC2_MAX 2016 \n" - "param set RC2_MIN 995\n" - "param set RC2_TRIM 1500\n" - "param set RC3_MAX 2003\n" - "param set RC3_MIN 992\n" - "param set RC3_TRIM 992\n" - "param set RC4_MAX 2011\n" - "param set RC4_MIN 997\n" - "param set RC4_TRIM 1504\n" - "param set RC4_REV -1\n" - "param set RC6_MAX 2016\n" - "param set RC6_MIN 992\n" - "param set RC6_TRIM 1504\n" - "param set RC_CHAN_CNT 8\n" - "param set RC_MAP_MODE_SW 5\n" - "param set RC_MAP_POSCTL_SW 7\n" - "param set RC_MAP_RETURN_SW 8\n" - "param set MC_YAW_P 1.5\n" - "param set MC_PITCH_P 3.0\n" - "param set MC_ROLL_P 3.0\n" - "param set MC_YAWRATE_P 0.2\n" - "param set MC_PITCHRATE_P 0.03\n" - "param set MC_ROLLRATE_P 0.03\n" - "param set ATT_W_ACC 0.0002\n" - "param set ATT_W_MAG 0.002\n" - "param set ATT_W_GYRO_BIAS 0.05\n" - "sleep 1\n" - - - "param set MAV_TYPE 2\n" - "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n" - "list_files\n" - "list_tasks\n" - "sleep 10\n" - "list_tasks\n" - "sleep 10\n" - - ; - - return commands; - -} diff --git a/platforms/qurt/src/px4/common/drv_hrt.cpp b/platforms/qurt/src/px4/common/drv_hrt.cpp deleted file mode 100644 index 0abb553d6c..0000000000 --- a/platforms/qurt/src/px4/common/drv_hrt.cpp +++ /dev/null @@ -1,477 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012 - 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_hrt.cpp - * - * High-resolution timer with callouts and timekeeping. - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include "hrt_work.h" - -// Intervals in usec -static constexpr unsigned HRT_INTERVAL_MIN = 50; -static constexpr unsigned HRT_INTERVAL_MAX = 50000000; - -/* - * Queue of callout entries. - */ -static struct sq_queue_s callout_queue; - -/* latency baseline (last compare value applied) */ -static uint64_t latency_baseline; - -/* timer count at interrupt (for latency purposes) */ -static uint64_t latency_actual; - -/* latency histogram */ -const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; -const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; - -static px4_sem_t _hrt_lock; -static struct work_s _hrt_work; - -static int32_t dsp_offset = 0; - -static void hrt_latency_update(); - -static void hrt_call_reschedule(); -static void hrt_call_invoke(); - -hrt_abstime hrt_absolute_time_offset() -{ - return 0; -} - -static void hrt_lock() -{ - px4_sem_wait(&_hrt_lock); -} - -static void hrt_unlock() -{ - px4_sem_post(&_hrt_lock); -} - -#include "dspal_time.h" - -int px4_clock_settime(clockid_t clk_id, struct timespec *tp) -{ - /* do nothing right now */ - return 0; -} - -int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) -{ - return clock_gettime(clk_id, tp); -} - -/* - * Get absolute time. - */ -hrt_abstime hrt_absolute_time() -{ - struct timespec ts; - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts) + dsp_offset; -} - -int hrt_set_absolute_time_offset(int32_t time_diff_us) -{ - dsp_offset = time_diff_us; - return 0; -} - -/* - * Convert a timespec to absolute time. - */ -hrt_abstime ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/* - * Compute the delta between a timestamp taken in the past - * and now. - * - * This function is safe to use even if the timestamp is updated - * by an interrupt during execution. - */ -hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - // This is not atomic as the value on the application layer of POSIX is limited. - hrt_abstime delta = hrt_absolute_time() - *then; - return delta; -} - -/* - * Store the absolute time in an interrupt-safe fashion. - * - * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. - */ -void hrt_store_absolute_time(volatile hrt_abstime *t) -{ - *t = hrt_absolute_time(); -} - -/* - * If this returns true, the entry has been invoked and removed from the callout list, - * or it has never been entered. - * - * Always returns false for repeating callouts. - */ -bool hrt_called(struct hrt_call *entry) -{ - return (entry->deadline == 0); -} - -/* - * Remove the entry from the callout list. - */ -void hrt_cancel(struct hrt_call *entry) -{ - hrt_lock(); - sq_rem(&entry->link, &callout_queue); - entry->deadline = 0; - - /* if this is a periodic call being removed by the callout, prevent it from - * being re-entered when the callout returns. - */ - entry->period = 0; - hrt_unlock(); - // endif -} - -static void hrt_latency_update() -{ - uint16_t latency = latency_actual - latency_baseline; - unsigned index; - - /* bounded buckets */ - for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { - if (latency <= latency_buckets[index]) { - latency_counters[index]++; - return; - } - } - - /* catch-all at the end */ - latency_counters[index]++; -} - -/* - * initialise a hrt_call structure - */ -void hrt_call_init(struct hrt_call *entry) -{ - memset(entry, 0, sizeof(*entry)); -} - -/* - * delay a hrt_call_every() periodic call by the given number of - * microseconds. This should be called from within the callout to - * cause the callout to be re-scheduled for a later time. The periodic - * callouts will then continue from that new base time at the - * previously specified period. - */ -void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) -{ - entry->deadline = hrt_absolute_time() + delay; -} - -/* - * Initialise the HRT. - */ -void hrt_init() -{ - sq_init(&callout_queue); - - int sem_ret = px4_sem_init(&_hrt_lock, 0, 1); - - if (sem_ret) { - PX4_ERR("SEM INIT FAIL: %s", strerror(errno)); - } - - memset(&_hrt_work, 0, sizeof(_hrt_work)); -} - -static void -hrt_call_enter(struct hrt_call *entry) -{ - struct hrt_call *call, *next; - - call = (struct hrt_call *)sq_peek(&callout_queue); - - if ((call == nullptr) || (entry->deadline < call->deadline)) { - sq_addfirst(&entry->link, &callout_queue); - //if (call != nullptr) PX4_INFO("call enter at head, reschedule (%lu %lu)", entry->deadline, call->deadline); - /* we changed the next deadline, reschedule the timer event */ - hrt_call_reschedule(); - - } else { - do { - next = (struct hrt_call *)sq_next(&call->link); - - if ((next == nullptr) || (entry->deadline < next->deadline)) { - //lldbg("call enter after head\n"); - sq_addafter(&call->link, &entry->link, &callout_queue); - break; - } - } while ((call = next) != nullptr); - } -} - -/** - * Timer interrupt handler - * - * This routine simulates a timer interrupt handler - */ -static void -hrt_tim_isr(void *p) -{ - /* grab the timer for latency tracking purposes */ - latency_actual = hrt_absolute_time(); - - /* do latency calculations */ - hrt_latency_update(); - - /* run any callouts that have met their deadline */ - hrt_call_invoke(); - - hrt_lock(); - - /* and schedule the next interrupt */ - hrt_call_reschedule(); - - hrt_unlock(); -} - -/** - * Reschedule the next timer interrupt. - * - * This routine must be called with interrupts disabled. - */ -static void -hrt_call_reschedule() -{ - hrt_abstime now = hrt_absolute_time(); - hrt_abstime delay = HRT_INTERVAL_MAX; - struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); - hrt_abstime deadline = now + HRT_INTERVAL_MAX; - - /* - * Determine what the next deadline will be. - * - * Note that we ensure that this will be within the counter - * period, so that when we truncate all but the low 16 bits - * the next time the compare matches it will be the deadline - * we want. - * - * It is important for accurate timekeeping that the compare - * interrupt fires sufficiently often that the base_time update in - * hrt_absolute_time runs at least once per timer period. - */ - if (next != nullptr) { - //lldbg("entry in queue\n"); - if (next->deadline <= (now + HRT_INTERVAL_MIN)) { - //lldbg("pre-expired\n"); - /* set a minimal deadline so that we call ASAP */ - delay = HRT_INTERVAL_MIN; - - } else if (next->deadline < deadline) { - //lldbg("due soon\n"); - delay = next->deadline - now; - } - } - - /* set the new compare value and remember it for latency tracking */ - latency_baseline = now + delay; - - // There is no timer ISR, so simulate one by putting an event on the - // high priority work queue - - // Remove the existing expiry and update with the new expiry - hrt_work_cancel(&_hrt_work); - - hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay); -} - -static void -hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) -{ - PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval); - hrt_lock(); - - //PX4_INFO("hrt_call_internal after lock"); - /* if the entry is currently queued, remove it */ - /* note that we are using a potentially uninitialised - entry->link here, but it is safe as sq_rem() doesn't - dereference the passed node unless it is found in the - list. So we potentially waste a bit of time searching the - queue for the uninitialised entry->link but we don't do - anything actually unsafe. - */ - if (entry->deadline != 0) { - sq_rem(&entry->link, &callout_queue); - } - - entry->deadline = deadline; - entry->period = interval; - entry->callout = callout; - entry->arg = arg; - - hrt_call_enter(entry); - hrt_unlock(); -} - -/* - * Call callout(arg) after delay has elapsed. - * - * If callout is nullptr, this can be used to implement a timeout by testing the call - * with hrt_called(). - */ -void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) -{ - //printf("hrt_call_after\n"); - hrt_call_internal(entry, - hrt_absolute_time() + delay, - 0, - callout, - arg); -} - -/* - * Call callout(arg) after delay, and then after every interval. - * - * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may - * jitter but should not drift. - */ -void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) -{ - hrt_call_internal(entry, - hrt_absolute_time() + delay, - interval, - callout, - arg); -} - -/* - * Call callout(arg) at absolute time calltime. - */ -void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) -{ - hrt_call_internal(entry, calltime, 0, callout, arg); -} - -static void -hrt_call_invoke() -{ - struct hrt_call *call; - hrt_abstime deadline; - - hrt_lock(); - - while (true) { - /* get the current time */ - hrt_abstime now = hrt_absolute_time(); - - call = (struct hrt_call *)sq_peek(&callout_queue); - - if (call == nullptr) { - break; - } - - if (call->deadline > now) { - break; - } - - sq_rem(&call->link, &callout_queue); - //PX4_INFO("call pop"); - - /* save the intended deadline for periodic calls */ - deadline = call->deadline; - - /* zero the deadline, as the call has occurred */ - call->deadline = 0; - - /* invoke the callout (if there is one) */ - if (call->callout) { - // Unlock so we don't deadlock in callback - hrt_unlock(); - - //PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg); - call->callout(call->arg); - - hrt_lock(); - } - - /* if the callout has a non-zero period, it has to be re-entered */ - if (call->period != 0) { - // re-check call->deadline to allow for - // callouts to re-schedule themselves - // using hrt_call_delay() - if (call->deadline <= now) { - call->deadline = deadline + call->period; - //PX4_INFO("call deadline set to %lu now=%lu", call->deadline, now); - } - - hrt_call_enter(call); - } - } - - hrt_unlock(); -} - -void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} diff --git a/platforms/qurt/src/px4/common/get_commands.h b/platforms/qurt/src/px4/common/get_commands.h deleted file mode 100644 index 2e89e67bbf..0000000000 --- a/platforms/qurt/src/px4/common/get_commands.h +++ /dev/null @@ -1,48 +0,0 @@ -/**************************************************************************** - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file get_commands.cpp - * functions to call to run the set of startup commands - * - * @author Mark Charlebois - */ - -#pragma once - -__BEGIN_DECLS -// The commands to run are specified in a target file: commands_.c -extern const char *get_commands(void); - -// Enable external library hook -void qurt_external_hook(void) __attribute__((weak)); -__END_DECLS - diff --git a/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h b/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h deleted file mode 100644 index d09b73c687..0000000000 --- a/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h +++ /dev/null @@ -1,36 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#pragma once - -#include - diff --git a/platforms/qurt/src/px4/common/lib_crc32.c b/platforms/qurt/src/px4/common/lib_crc32.c deleted file mode 100644 index c14ebfceeb..0000000000 --- a/platforms/qurt/src/px4/common/lib_crc32.c +++ /dev/null @@ -1,125 +0,0 @@ -/************************************************************************************************ - * libc/misc/lib_crc32.c - * - * This file is a part of NuttX: - * - * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. - * - * The logic in this file was developed by Gary S. Brown: - * - * COPYRIGHT (C) 1986 Gary S. Brown. You may use this program, or code or tables - * extracted from it, as desired without restriction. - * - * First, the polynomial itself and its table of feedback terms. The polynomial is: - * - * X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0 - * - * Note that we take it "backwards" and put the highest-order term in the lowest-order bit. - * The X^32 term is "implied"; the LSB is the X^31 term, etc. The X^0 term (usually shown - * as "+1") results in the MSB being 1 - * - * Note that the usual hardware shift register implementation, which is what we're using - * (we're merely optimizing it by doing eight-bit chunks at a time) shifts bits into the - * lowest-order term. In our implementation, that means shifting towards the right. Why - * do we do it this way? Because the calculated CRC must be transmitted in order from - * highest-order term to lowest-order term. UARTs transmit characters in order from LSB - * to MSB. By storing the CRC this way we hand it to the UART in the order low-byte to - * high-byte; the UART sends each low-bit to hight-bit; and the result is transmission bit - * by bit from highest- to lowest-order term without requiring any bit shuffling on our - * part. Reception works similarly - * - * The feedback terms table consists of 256, 32-bit entries. Notes - * - * - The table can be generated at runtime if desired; code to do so is shown later. It - * might not be obvious, but the feedback terms simply represent the results of eight - * shift/xor operations for all combinations of data and CRC register values - * - * - The values must be right-shifted by eight bits by the updcrc logic; the shift must - * be u_(bring in zeroes). On some hardware you could probably optimize the shift in - * assembler by using byte-swap instructions polynomial $edb88320 - ************************************************************************************************/ - -/************************************************************************************************ - * Included Files - ************************************************************************************************/ - -#include -#include -#include - -// Needed for Linux -#define FAR - -/************************************************************************************************ - * Private Data - ************************************************************************************************/ - -static const uint32_t crc32_tab[] = { - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -/************************************************************************************************ - * Public Functions - ************************************************************************************************/ -/************************************************************************************************ - * Name: crc32part - * - * Description: - * Continue CRC calculation on a part of the buffer. - * - ************************************************************************************************/ - -uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) -{ - size_t i; - - for (i = 0; i < len; i++) { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - - return crc32val; -} - -/************************************************************************************************ - * Name: crc32 - * - * Description: - * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' - * - ************************************************************************************************/ - -uint32_t crc32(FAR const uint8_t *src, size_t len) -{ - return crc32part(src, len, 0); -} diff --git a/platforms/qurt/src/px4/common/main.cpp b/platforms/qurt/src/px4/common/main.cpp deleted file mode 100644 index a238dc33f5..0000000000 --- a/platforms/qurt/src/px4/common/main.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/**************************************************************************** - * Copyright (C) 2015-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file main.cpp - * - * Basic shell to execute builtin "apps" on QURT. - * - * @author Mark Charlebois - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "get_commands.h" -#include "apps.h" - -#define MAX_ARGS 8 // max number of whitespace separated args after app name - - -static px4_task_t g_dspal_task = -1; - -__BEGIN_DECLS -// The commands to run are specified in a target file: commands_.c -extern const char *get_commands(void); - -// Enable external library hook -void qurt_external_hook(void) __attribute__((weak)); -__END_DECLS - -void qurt_external_hook(void) -{ -} - -static void run_cmd(apps_map_type &apps, const std::vector &appargs) -{ - // command is appargs[0] - std::string command = appargs[0]; - - //replaces app.find with iterator code to avoid null pointer exception - for (apps_map_type::iterator it = apps.begin(); it != apps.end(); ++it) - if (it->first == command) { - // one for command name, one for null terminator - const char *arg[MAX_ARGS + 2]; - - unsigned int i = 0; - - if (appargs.size() > MAX_ARGS + 1) { - PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1)); - return; - } - - while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { - arg[i] = (char *)appargs[i].c_str(); - PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]); - ++i; - } - - arg[i] = (char *)0; - - //PX4_DEBUG_PRINTF(i); - if (apps[command] == NULL) { - PX4_ERR("Null function !!\n"); - - } else { - apps[command](i, (char **)arg); - break; - } - - } -} - -void eat_whitespace(const char *&b, int &i) -{ - // Eat whitespace - while (b[i] == ' ' || b[i] == '\t') { ++i; } - - b = &b[i]; - i = 0; -} - -static void process_commands(apps_map_type &apps, const char *cmds) -{ - std::vector appargs; - int i = 0; - const char *b = cmds; - char arg[256]; - - // Eat leading whitespace - eat_whitespace(b, i); - - for (;;) { - // End of command line - if (b[i] == '\n' || b[i] == '\0') { - strncpy(arg, b, i); - arg[i] = '\0'; - appargs.push_back(arg); - - // If we have a command to run - if (appargs.size() > 0) { - PX4_DEBUG("Processing command: %s", appargs[0].c_str()); - - for (int ai = 1; ai < (int)appargs.size(); ai++) { - PX4_DEBUG(" > arg: %s", appargs[ai].c_str()); - } - - run_cmd(apps, appargs); - } - - appargs.clear(); - - if (b[i] == '\n') { - eat_whitespace(b, ++i); - continue; - - } else { - break; - } - } - - // End of arg - else if (b[i] == ' ') { - strncpy(arg, b, i); - arg[i] = '\0'; - appargs.push_back(arg); - eat_whitespace(b, ++i); - continue; - } - - ++i; - } -} - -namespace px4 -{ -extern void init_once(void); -}; - -__BEGIN_DECLS -int dspal_main(int argc, char *argv[]); -__END_DECLS - - -const char *get_commands() -{ - // All that needs to be started automatically on the DSP side - // are uorb and qshell. After that, everything else can get - // started from the main startup script on the Linux side. - static const char *commands = "qshell start\n"; - - return commands; -} - - -int dspal_entry(int argc, char *argv[]) -{ - PX4_INFO("In dspal_entry"); - apps_map_type apps; - init_app_map(apps); - px4::init_once(); - px4::init(argc, (char **)argv, "px4"); - process_commands(apps, get_commands()); - sleep(1); // give time for all commands to execute before starting external function - - if (qurt_external_hook) { - qurt_external_hook(); - } - - for (;;) { - sleep(1); - } - - return 0; -} - -static void usage() -{ - PX4_INFO("Usage: dspal {start |stop}"); -} - - -extern "C" { - - int dspal_main(int argc, char *argv[]) - { - int ret = 0; - - if (argc == 2 && strcmp(argv[1], "start") == 0) { - g_dspal_task = px4_task_spawn_cmd("dspal", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - dspal_entry, - argv); - - } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_dspal_task < 0) { - PX4_WARN("start up thread not running"); - - } else { - px4_task_delete(g_dspal_task); - g_dspal_task = -1; - } - - } else { - usage(); - ret = -1; - } - - return ret; - } -} diff --git a/platforms/qurt/src/px4/common/px4_init.cpp b/platforms/qurt/src/px4/common/px4_init.cpp deleted file mode 100644 index 73bada3ae1..0000000000 --- a/platforms/qurt/src/px4/common/px4_init.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include - -int px4_platform_init() -{ - hrt_init(); - - param_init(); - - px4::WorkQueueManagerStart(); - - uorb_start(); - - //px4_log_initialize(); - - return PX4_OK; -} diff --git a/platforms/qurt/src/px4/common/px4_qurt_impl.cpp b/platforms/qurt/src/px4/common/px4_qurt_impl.cpp deleted file mode 100644 index 1f8825d59c..0000000000 --- a/platforms/qurt/src/px4/common/px4_qurt_impl.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_linux_impl.cpp - * - * PX4 Middleware Wrapper Linux Implementation - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "hrt_work.h" - -//extern pthread_t _shell_task_id; - - -__BEGIN_DECLS -extern uint64_t get_ticks_per_us(); - -//long PX4_TICKS_PER_SEC = 1000L; - -unsigned int sleep(unsigned int sec) -{ - for (unsigned int i = 0; i < sec; i++) { - usleep(1000000); - } - - return 0; -} - -extern void hrt_init(void); - -#if 0 -void qurt_log(const char *fmt, ...) -{ - va_list args; - va_start(args, fmt); - printf(fmt, args); - printf("n"); - va_end(args); -} -#endif - -//extern int _posix_init(void); - -__END_DECLS - -extern struct wqueue_s gwork[NWORKERS]; - - -namespace px4 -{ - -void init_once(void); - -void init_once(void) -{ - // Required for QuRT - //_posix_init(); - -// _shell_task_id = pthread_self(); -// PX4_INFO("Shell id is %lu", _shell_task_id); - - work_queues_init(); - hrt_work_queue_init(); - - px4_platform_init(); -} - -void init(int argc, char *argv[], const char *app_name) -{ - PX4_DEBUG("App name: %s\n", app_name); -} - -} - -/** Retrieve from the data manager store */ -ssize_t -dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -) -{ - return 0; -} - -/** write to the data manager store */ -ssize_t -dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - dm_persitence_t persistence, /* The persistence level of this item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -) -{ - return 0; -} - -size_t strnlen(const char *s, size_t maxlen) -{ - size_t i = 0; - - while (s[i] != '\0' && i < maxlen) { - ++i; - } - - return i; -} - -int fprintf(FILE *stream, const char *format, ...) -{ - PX4_ERR("Error: Calling unresolved symbol stub:[%s(%s,...)]", __FUNCTION__, format); - return 0; -} - -int fputc(int c, FILE *stream) -{ - return c; -} - -int putchar(int character) -{ - return character; -} diff --git a/platforms/qurt/src/px4/common/qurt_stubs.c b/platforms/qurt/src/px4/common/qurt_stubs.c deleted file mode 100644 index 77f8737759..0000000000 --- a/platforms/qurt/src/px4/common/qurt_stubs.c +++ /dev/null @@ -1,155 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include -#include - -void block_indefinite(void); -void _Read_uleb(void); -void _Parse_fde_instr(void); -void _Parse_csd(void); -void _Valbytes(void); -void _Get_eh_data(void); -void _Parse_lsda(void); -void __cxa_guard_release(void); -void _Read_enc_ptr(void); -void _Read_sleb(void); -void __cxa_guard_acquire(void); -void __cxa_pure_virtual(void); - -void block_indefinite(void) -{ - sem_t forever; - sem_init(&forever, 0, 0); - sem_wait(&forever); -} - -void _Read_uleb(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Parse_fde_instr(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Parse_csd(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Valbytes(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Get_eh_data(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Parse_lsda(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void __cxa_guard_release(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Read_enc_ptr(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Read_sleb(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void __cxa_guard_acquire(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void __cxa_pure_virtual() -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -float _Stof(const char *p0, char **p1, long p2) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); - return 0; -} - -void *bsearch(const void *key, const void *ptr, size_t count, size_t size, int (*comp)(const void *, const void *)) -{ - const char *first = (const char *)ptr; - - while (count > 1) { - size_t m = count / 2; - const char *middle_element = first + m * size; - int cmp_res = comp(middle_element, key); - - if (cmp_res > 0) { - count = m; - - } else if (cmp_res == 0) { - return (void *)middle_element; - - } else { - first = middle_element + size; - count = count - m - 1; - } - } - - if (count && comp(first, key) == 0) { - return (void *)first; - } - - return NULL; -} diff --git a/platforms/qurt/src/px4/common/shmem_qurt.cpp b/platforms/qurt/src/px4/common/shmem_qurt.cpp deleted file mode 100644 index cb9ed77d36..0000000000 --- a/platforms/qurt/src/px4/common/shmem_qurt.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -//#define SHMEM_DEBUG -//#define PARAM_LOCK_DEBUG - -static atomic_word_t mem_lock; - -static unsigned char *map_base, *virt_addr; -static uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; -static unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; - -struct shmem_info *shmem_info_p; - -// Small helper to get log2 for ints -static unsigned log2_for_int(unsigned v) -{ - unsigned r = 0; - - while (v >>= 1) { - ++r; - } - - return r; -} - -struct param_wbuf_s { - union param_value_u val; - param_t param; - bool unsaved; -}; -extern struct param_wbuf_s *param_find_changed(param_t param); - -int get_shmem_lock(const char *caller_file_name, int caller_line_number) -{ - unsigned int i = 0; - -#ifdef PARAM_LOCK_DEBUG - PX4_INFO("lock value %d before get from %s, line: %d\n", mem_lock.value, strrchr(caller_file_name, '/'), - caller_line_number); -#endif - - while (!atomic_compare_and_set(&mem_lock, 1, 0)) { - i++; - usleep(1000); - - if (i > 100) { - break; - } - } - - if (i > 100) { - PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", - strrchr(caller_file_name, '/'), caller_line_number); - return -1; - - } else { - PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", - caller_file_name, caller_line_number); - } - - return 0; //got the lock - -} - -void release_shmem_lock(const char *caller_file_name, int caller_line_number) -{ - atomic_set(&mem_lock, 1); - -#ifdef PARAM_LOCK_DEBUG - PX4_INFO("release lock, file name: %s, line number: %d.\n", - strrchr(caller_file_name, '/'), caller_line_number); -#endif - - return; -} - -void init_shared_memory(void) -{ - int i; - - if (shmem_info_p) { - return; - } - - //virt_addr = map_memory(MAP_ADDRESS); - map_base = (unsigned char *)calloc(MAP_SIZE, 1); //16KB - - if (map_base == NULL) { - PX4_INFO("adsp memory malloc failed\n"); - return; - } - - virt_addr = map_base; - shmem_info_p = (struct shmem_info *) virt_addr; - - atomic_init(&mem_lock, 1); - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - shmem_info_p->krait_changed_index[i] = 0; - } - - PX4_INFO("adsp memory mapped\n"); -} - -void copy_params_to_shmem(const param_info_s *param_info_base) -{ - param_t param; - unsigned int i; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_INFO("Could not get shmem lock\n"); - return; - } - - //else PX4_INFO("Got lock\n"); - - for (param = 0; param < param_count(); param++) { - //{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);} - struct param_wbuf_s *s = param_find_changed(param); - - if (s == NULL) { - shmem_info_p->params_val[param] = param_info_base[param].val; - } - - else { - shmem_info_p->params_val[param] = s->val; - } - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param)); - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param)); - } - -#endif - } - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - shmem_info_p->adsp_changed_index[i] = 0; - krait_changed_index[i] = 0; - } - - release_shmem_lock(__FILE__, __LINE__); - //PX4_INFO("Released lock\n"); - -} - -/*update value and param's change bit in shared memory*/ -void update_to_shmem(param_t param, union param_value_u value) -{ - unsigned int byte_changed, bit_changed; - - if (!handle_in_range(param)) { - return; - } - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock\n"); - return; - } - - shmem_info_p->params_val[param] = value; - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->adsp_changed_index[byte_changed] |= bit_changed; - - //PX4_INFO("set %d bit on adsp index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->adsp_changed_index[byte_changed]); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, - bit_changed); - } - - else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, - bit_changed); - } - -#endif - - release_shmem_lock(__FILE__, __LINE__); - -} - -void update_index_from_shmem(void) -{ - unsigned int i; - param_t params[MAX_SHMEM_PARAMS / 8 + 1]; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock\n"); - return; - } - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - // Check if any param has been changed. - if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) { - - // If a param has changed, we need to find out which one. - // From the byte and bit that is different, we can resolve the param number. - unsigned bit = log2_for_int( - krait_changed_index[i] - ^ shmem_info_p->krait_changed_index[i]); - param_t param_to_get = i * 8 + bit; - - // Update our krait_changed_index as well. - krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; - params[i] = param_to_get; - - } else { - params[i] = 0xFFFF; - } - } - - release_shmem_lock(__FILE__, __LINE__); - - // FIXME: this is a hack but it gets the param so that it gets added - // to the local list param_values in param_shmem.c. - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - if (params[i] != 0xFFFF) { - int32_t dummy; - param_get(params[i], &dummy); - } - } -} - -static void update_value_from_shmem(param_t param, union param_value_u *value) -{ - unsigned int byte_changed, bit_changed; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock\n"); - return; - } - - *value = shmem_info_p->params_val[param]; - - /*also clear the index since we are holding the lock*/ - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; - - release_shmem_lock(__FILE__, __LINE__); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, - bit_changed); - } - - else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, - bit_changed); - } - -#endif -} - -int update_from_shmem(param_t param, union param_value_u *value) -{ - unsigned int byte_changed, bit_changed; - unsigned int retval = 0; - - if (!handle_in_range(param) || value == NULL) { - return retval; - } - - update_from_shmem_current_time = hrt_absolute_time(); - - if ((update_from_shmem_current_time - update_from_shmem_prev_time) - > 1000000) { //update every 1 second - update_from_shmem_prev_time = update_from_shmem_current_time; - update_index_from_shmem(); - } - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - - if (krait_changed_index[byte_changed] & bit_changed) { - update_value_from_shmem(param, value); - krait_changed_index[byte_changed] &= ~bit_changed; - retval = 1; - } - - //else {PX4_INFO("no change to param %s\n", param_name(param));} - - PX4_DEBUG("%s %d bit on krait changed index[%d]\n", - (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); - - return retval; -} diff --git a/platforms/qurt/src/px4/common/stubs/posix.c b/platforms/qurt/src/px4/common/stubs/posix.c deleted file mode 100644 index 24b22f13f9..0000000000 --- a/platforms/qurt/src/px4/common/stubs/posix.c +++ /dev/null @@ -1,159 +0,0 @@ -#include -#include -#include -#include -#include - -int sem_init(sem_t *sem, int pshared, unsigned int value) -{ - return 1; -} - -int sem_wait(sem_t *sem) -{ - return 1; -} - -int sem_destroy(sem_t *sem) -{ - return 1; -} - -int sem_post(sem_t *sem) -{ - return 1; -} - -int sem_getvalue(sem_t *sem, int *sval) -{ - return 1; -} - -int usleep(useconds_t usec) -{ - return 0; -} - -pthread_t pthread_self(void) -{ - pthread_t x = 0; - return x; -} - - -int pthread_kill(pthread_t thread, int sig) -{ - return 1; -} - -void pthread_exit(void *retval) -{ -} - -int pthread_join(pthread_t thread, void **retval) -{ - return 1; -} - -int pthread_cancel(pthread_t thread) -{ - return 1; -} -int pthread_attr_init(pthread_attr_t *attr) -{ - return 1; -} - -int pthread_attr_setstacksize(pthread_attr_t *attr, size_t stacksize) -{ - return 1; -} - -int pthread_attr_getstacksize(const pthread_attr_t *attr, size_t *stacksize) -{ - return 1; -} - -int pthread_attr_setschedparam(pthread_attr_t *attr, const struct sched_param *param) -{ - return 1; -} - -int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) -{ - return 1; -} -int pthread_attr_getschedparam(const pthread_attr_t *attr, struct sched_param *param) -{ - return 1; -} - -int pthread_attr_destroy(pthread_attr_t *attr) -{ - return 1; -} - -int clock_gettime(clockid_t clk_id, struct timespec *tp) -{ - return 1; -} - -int pthread_mutex_lock(pthread_mutex_t *mutex) -{ - return 1; -} - -int pthread_mutex_unlock(pthread_mutex_t *mutex) -{ - return 1; -} - -int pthread_cond_signal(pthread_cond_t *cond) -{ - return 1; -} -int pthread_mutex_destroy(pthread_mutex_t *mutex) -{ - return 1; -} -int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr) -{ - return 1; -} - -int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) -{ - return 1; -} - -int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime) -{ - return 1; -} - -int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr) -{ - return 1; -} -int pthread_mutexattr_init(pthread_mutexattr_t *attr) -{ - return -1; -} -int pthread_mutexattr_destroy(pthread_mutexattr_t *attr) -{ - return -1; -} -int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type) -{ - return -1; -} - -int pthread_condattr_init(pthread_condattr_t *attr) -{ - return -1; -} - -int fsync(int fd) -{ - return -1; -} diff --git a/platforms/qurt/src/px4/common/stubs/qurt.c b/platforms/qurt/src/px4/common/stubs/qurt.c deleted file mode 100644 index 09a28f52b4..0000000000 --- a/platforms/qurt/src/px4/common/stubs/qurt.c +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include -#include -#include - -// This code is never run. It is used solely to test linking in the -// TravisCI build test to make sure all symbols are resolved. -__EXPORT int _start_main(void); - -void HAP_debug(const char *msg, int level, const char *filename, int line) -{ -} - -void HAP_power_request(int a, int b, int c) -{ -} - -int dlinit(int a, char **b) -{ - return 1; -} - -int main(int argc, char *argv[]) -{ - int ret = 0; - - return ret; -} - -int _start_main() -{ - return -1; -} diff --git a/platforms/qurt/src/px4/common/tasks.cpp b/platforms/qurt/src/px4/common/tasks.cpp deleted file mode 100644 index 2ec1d1c4db..0000000000 --- a/platforms/qurt/src/px4/common/tasks.cpp +++ /dev/null @@ -1,402 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015-2016 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_qurt_tasks.c - * Implementation of existing task API for QURT. - * - * @author Mark Charlebois - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if !defined(__PX4_QURT) -#include -#endif - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#define MAX_CMD_LEN 100 - -#define PX4_MAX_TASKS 50 -#define SHELL_TASK_ID (PX4_MAX_TASKS+1) - -pthread_t _shell_task_id = 0; - -struct task_entry { - pthread_t pid; - std::string name; - bool isused; - task_entry() : isused(false) {} -}; - -static task_entry taskmap[PX4_MAX_TASKS]; - -typedef struct { - px4_main_t entry; - int argc; - char *argv[]; - // strings are allocated after the -} pthdata_t; - -static void *entry_adapter(void *ptr) -{ - pthdata_t *data; - data = (pthdata_t *) ptr; - - data->entry(data->argc, data->argv); - //PX4_WARN("Before waiting infinte busy loop"); - //for( ;; ) - //{ - // volatile int x = 0; - // ++x; - // } - free(ptr); - px4_task_exit(0); - - return NULL; -} - -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, - char *const argv[]) -{ - struct sched_param param; - pthread_attr_t attr; - pthread_t task; - int rv; - int argc = 0; - int i; - unsigned int len = 0; - unsigned long offset; - unsigned long structsize; - char *p = (char *)argv; - - PX4_DEBUG("Creating %s\n", name); - PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m); - - // Calculate argc - while (p != (char *)0) { - p = argv[argc]; - - if (p == (char *)0) { - break; - } - - ++argc; - len += strlen(p) + 1; - } - - structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); - pthdata_t *taskdata = nullptr; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize + len); - - if (taskdata == nullptr) { - return -ENOMEM; - } - - offset = ((unsigned long)taskdata) + structsize; - - taskdata->entry = entry; - taskdata->argc = argc; - - for (i = 0; i < argc; i++) { - PX4_DEBUG("arg %d %s\n", i, argv[i]); - taskdata->argv[i] = (char *)offset; - strcpy((char *)offset, argv[i]); - offset += strlen(argv[i]) + 1; - } - - // Must add NULL at end of argv - taskdata->argv[argc] = (char *)0; - - rv = pthread_attr_init(&attr); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); - return (rv < 0) ? rv : -rv; - } - - PX4_DEBUG("stack address after pthread_attr_init: 0x%X", attr.stackaddr); - PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m); - rv = pthread_attr_getschedparam(&attr, ¶m); - PX4_DEBUG("stack address after pthread_attr_getschedparam: 0x%X", attr.stackaddr); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param"); - return (rv < 0) ? rv : -rv; - } - -#if 0 - rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); - return (rv < 0) ? rv : -rv; - } - - rv = pthread_attr_setschedpolicy(&attr, scheduler); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); - return (rv < 0) ? rv : -rv; - } - -#endif - size_t fixed_stacksize = -1; - pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_DEBUG("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); - fixed_stacksize = 8 * 1024; - fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; - - PX4_DEBUG("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); - pthread_attr_setstacksize(&attr, PX4_STACK_ADJUSTED(fixed_stacksize)); - - PX4_DEBUG("stack address after pthread_attr_setstacksize: 0x%X", attr.stackaddr); - param.sched_priority = priority; - - rv = pthread_attr_setschedparam(&attr, ¶m); - - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: failed to set sched param"); - return (rv < 0) ? rv : -rv; - } - - rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); - - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: pthread_create failed, error: %d", rv); - return (rv < 0) ? rv : -rv; - } - - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused == false) { - taskmap[i].pid = task; - taskmap[i].name = name; - taskmap[i].isused = true; - break; - } - } - - if (i >= PX4_MAX_TASKS) { - return -ENOSPC; - } - - return i; -} - -int px4_task_delete(px4_task_t id) -{ - int rv = 0; - pthread_t pid; - PX4_WARN("Called px4_task_delete"); - - if (id < PX4_MAX_TASKS && taskmap[id].isused) { - pid = taskmap[id].pid; - - } else { - return -EINVAL; - } - - // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { - taskmap[id].isused = false; - pthread_exit(0); - - } else { - rv = pthread_cancel(pid); - } - - taskmap[id].isused = false; - - return rv; -} - -void px4_task_exit(int ret) -{ - int i; - pthread_t pid = pthread_self(); - - // Get pthread ID from the opaque ID - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].pid == pid) { - taskmap[i].isused = false; - break; - } - } - - if (i >= PX4_MAX_TASKS) { - PX4_ERR("px4_task_exit: self task not found!"); - - } else { - PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); - } - - //pthread_exit((void *)(unsigned long)ret); -} - -int px4_task_kill(px4_task_t id, int sig) -{ - int rv = 0; - pthread_t pid; - PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); - - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) { - pid = taskmap[id].pid; - - } else { - return -EINVAL; - } - - // If current thread then exit, otherwise cancel - rv = pthread_kill(pid, sig); - - return rv; -} - -void px4_show_tasks() -{ - int idx; - int count = 0; - - PX4_INFO("Active Tasks:"); - - for (idx = 0; idx < PX4_MAX_TASKS; idx++) { - if (taskmap[idx].isused) { - PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); - count++; - } - } - - if (count == 0) { - PX4_INFO(" No running tasks"); - } - -} - -px4_task_t px4_getpid() -{ - pthread_t pid = pthread_self(); -// -// if (pid == _shell_task_id) -// return SHELL_TASK_ID; - - // Get pthread ID from the opaque ID - for (int i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused && taskmap[i].pid == pid) { - return i; - } - } - - return ~0; -} - - -const char *px4_get_taskname() -{ - pthread_t pid = pthread_self(); - - for (int i = 0; i < PX4_MAX_TASKS; i++) { - if (taskmap[i].isused && taskmap[i].pid == pid) { - return taskmap[i].name.c_str(); - } - } - - return "Unknown App"; -} - -static void timer_cb(void *data) -{ - px4_sem_t *sem = reinterpret_cast(data); - - sem_post(sem); -} - -int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) -{ - work_s _hpwork = {}; - - // Get the current time. - struct timespec ts_now; - px4_clock_gettime(CLOCK_MONOTONIC, &ts_now); - - // We get an absolute time but want to calculate a timeout in us. - hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now); - - // Create a timer to unblock. - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us); - sem_wait(sem); - hrt_work_cancel(&_hpwork); - return 0; -} - -int px4_prctl(int option, const char *arg2, px4_task_t pid) -{ - int rv; - - switch (option) { - case PR_SET_NAME: - // set the threads name - Not supported - // rv = pthread_setname_np(pthread_self(), arg2); - rv = -1; - break; - - default: - rv = -1; - PX4_WARN("FAILED SETTING TASK NAME"); - break; - } - - return rv; -} diff --git a/platforms/qurt/src/px4/generic/CMakeLists.txt b/platforms/qurt/src/px4/generic/CMakeLists.txt deleted file mode 100644 index ec489bf559..0000000000 --- a/platforms/qurt/src/px4/generic/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - - -add_subdirectory(${PX4_CHIP}) - diff --git a/platforms/qurt/src/px4/generic/generic/CMakeLists.txt b/platforms/qurt/src/px4/generic/generic/CMakeLists.txt deleted file mode 100644 index c4e98943e4..0000000000 --- a/platforms/qurt/src/px4/generic/generic/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - - - diff --git a/platforms/qurt/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h b/platforms/qurt/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h deleted file mode 100644 index 82e42c1882..0000000000 --- a/platforms/qurt/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - - -static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) -{ - px4_i2c_bus_t ret{}; - ret.bus = bus; - ret.is_external = false; - return ret; -} - -static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) -{ - px4_i2c_bus_t ret{}; - ret.bus = bus; - ret.is_external = true; - return ret; -} diff --git a/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h b/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h deleted file mode 100644 index d09b73c687..0000000000 --- a/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h +++ /dev/null @@ -1,36 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#pragma once - -#include - diff --git a/platforms/qurt/src/px4/generic/generic/include/px4_arch/spi_hw_description.h b/platforms/qurt/src/px4/generic/generic/include/px4_arch/spi_hw_description.h deleted file mode 100644 index 52836a1c70..0000000000 --- a/platforms/qurt/src/px4/generic/generic/include/px4_arch/spi_hw_description.h +++ /dev/null @@ -1,59 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -static inline constexpr px4_spi_bus_device_t initSPIDevice(uint8_t devid_driver) -{ - px4_spi_bus_device_t ret{}; - ret.cs_gpio = 1; // set to some non-zero value to indicate this is used - ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, 0); - ret.devtype_driver = devid_driver; - return ret; -} - -static inline constexpr px4_spi_bus_t initSPIBus(int bus, const px4_spi_bus_devices_t &devices) -{ - px4_spi_bus_t ret{}; - - for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { - ret.devices[i] = devices.devices[i]; - } - - ret.bus = bus; - ret.is_external = false; // all buses are marked internal on QuRT - ret.requires_locking = false; - return ret; -} diff --git a/posix-configs/eagle/200qx/mainapp.config b/posix-configs/eagle/200qx/mainapp.config deleted file mode 100644 index 251eadb0bc..0000000000 --- a/posix-configs/eagle/200qx/mainapp.config +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -t -b 200 -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config deleted file mode 100644 index 90d35c64e8..0000000000 --- a/posix-configs/eagle/200qx/px4.config +++ /dev/null @@ -1,38 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set SENS_BOARD_ROT 0 - - -sleep 1 -mpu9250 -s start -bmp280 -I start -gps start -d /dev/tty-4 -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -uart_esc start -D /dev/tty-2 -spektrum_rc start -d /dev/tty-1 -sleep 1 -list_files -list_tasks diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config deleted file mode 100644 index 251eadb0bc..0000000000 --- a/posix-configs/eagle/210qc/mainapp.config +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -t -b 200 -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config deleted file mode 100644 index 941a704607..0000000000 --- a/posix-configs/eagle/210qc/px4.config +++ /dev/null @@ -1,38 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set ATT_W_MAG 0.00 -param set SENS_BOARD_ROT 0 - -sleep 1 -mpu9250 -s start -bmp280 -I start -gps start -d /dev/tty-4 -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -uart_esc start -D /dev/tty-2 -spektrum_rc start -d /dev/tty-1 -sleep 1 -list_files -list_tasks diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config deleted file mode 100644 index 251eadb0bc..0000000000 --- a/posix-configs/eagle/flight/mainapp.config +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -t -b 200 -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config deleted file mode 100644 index de030a0217..0000000000 --- a/posix-configs/eagle/flight/px4.config +++ /dev/null @@ -1,24 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -sleep 1 -gps start -d /dev/tty-4 -param set MAV_TYPE 2 -sleep 1 -hmc5883 start -mpu9250 start -bmp280 start -sleep 1 -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -# TODO -#linux_pwm_out start -spektrum_rc start diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config deleted file mode 100644 index b61bd58bbc..0000000000 --- a/posix-configs/eagle/hil/mainapphil.config +++ /dev/null @@ -1,11 +0,0 @@ -muorb start -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -mavlink start -x -u 14556 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink boot_complete -battery_simulator start -simulator start diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config deleted file mode 100644 index f1afab09db..0000000000 --- a/posix-configs/eagle/hil/px4.config +++ /dev/null @@ -1,32 +0,0 @@ -qshell start - -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2 -param set MC_YAW_P 1.5 -param set MC_PITCH_P 3.0 -param set MC_ROLL_P 3.0 -param set MC_YAWRATE_P 0.2 -param set MC_PITCHRATE_P 0.03 -param set MC_ROLLRATE_P 0.03 - -sleep 1 -rc_update start -commander start -hil -sensors start -ekf2 start -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -land_detector start multicopter -sleep 1 -pwm_out_sim start -mixer load /dev/pwm_output0 /startup/quad_x.main.mix -list_files -list_tasks -sleep 10 -list_tasks -sleep 10 diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS deleted file mode 100644 index f8a0dca0c8..0000000000 --- a/posix-configs/eagle/init/rcS +++ /dev/null @@ -1,103 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start - -# We need to wait until the DSP side is ready before -# sending commands with qshell. -sleep 1 - -# default, generic quad platform -if param compare SYS_AUTOSTART 4100 -then - param set MAV_TYPE 2 - - qshell pwm_out_rc_in start -d /dev/tty-2 - - qshell gps start -d /dev/tty-4 - qshell hmc5883 start - -# Qualcomm 200qx microheli platform -elif param compare SYS_AUTOSTART 4200 -then - param set MAV_TYPE 2 - param set MC_YAW_P 7.0 - param set MC_YAWRATE_P 0.08 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.0001 - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.08 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.0001 - param set SENS_BOARD_ROT 0 - -# Qualcomm internal 210qc platform -elif param compare SYS_AUTOSTART 4210 -then - param set MAV_TYPE 2 - param set MC_YAW_P 12 - param set MC_YAWRATE_P 0.08 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.001 - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.08 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.001 - param set SENS_BOARD_ROT 0 - -else - echo "NO AIRFRAME CHOSEN!" - echo "Please set parameter to select airframe:" - echo " SYS_AUTOSTART 4100: generic (flight) Quad" - echo " SYS_AUTOSTART 4200: Microheli 200QX" -fi - - -qshell mpu9250 start -qshell bmp280 start -qshell rc_update start -qshell sensors start - -if param compare SYS_MC_EST_GROUP 1 -then - qshell attitude_estimator_q start - qshell local_position_estimator start - -elif param compare SYS_MC_EST_GROUP 2 -then - param set EKF2_GBIAS_INIT 0.01 - param set EKF2_ANGERR_INIT 0.01 - qshell ekf2 start -else - echo "No estimator chosen" - exit -1 -fi - -qshell commander start -qshell land_detector start multicopter -qshell mc_hover_thrust_estimator start -qshell flight_mode_manager start -qshell mc_pos_control start -qshell mc_att_control start -qshell mc_rate_control start - -logger start -b 200 -t - -dataman start -navigator start -mavlink start -u 14556 -r 1000000 -p -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/excelsior/mainapp.config b/posix-configs/excelsior/mainapp.config deleted file mode 100644 index 4146bfd4e4..0000000000 --- a/posix-configs/excelsior/mainapp.config +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/sh -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -e -t -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -sleep 2 -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config deleted file mode 100644 index f634095b35..0000000000 --- a/posix-configs/excelsior/px4.config +++ /dev/null @@ -1,36 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set SENS_BOARD_ROT 4 - -sleep 1 -mpu9250 -s start -bmp280 -I start -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -uart_esc start -D /dev/tty-1 -spektrum_rc start -d /dev/tty-101 -sleep 1 -list_files -list_tasks diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config deleted file mode 100644 index 31d8482b1a..0000000000 --- a/posix-configs/ocpoc/px4.config +++ /dev/null @@ -1,50 +0,0 @@ -#!/bin/sh -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -param select /root/rootfs/eeprom/parameters -if [ -f /root/rootfs/eeprom/parameters ] -then - param load -fi -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MAV_SYS_ID 1 - -load_mon start - -mpu9250 -s -R 4 start -hmc5883 -I start -ms5611 -s start - -rgbled start -b 1 - -board_adc start -battery_status start - -gps start -d /dev/ttyS3 -s -rc_update start -sensors start -commander start -navigator start -dataman start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start - -mavlink start -x -d /dev/ttyPS1 -mavlink stream -d /dev/ttyPS1 -s HIGHRES_IMU -r 50 -mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50 - -rc_input start -d /dev/ttyS2 - -linux_pwm_out start -mixer load /dev/pwm_out etc/mixers/quad_x.main.mix -logger start -t -b 200 -mavlink boot_complete diff --git a/src/drivers/qshell/posix/CMakeLists.txt b/src/drivers/qshell/posix/CMakeLists.txt deleted file mode 100644 index d028be7983..0000000000 --- a/src/drivers/qshell/posix/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE drivers__qshell__posix - MAIN qshell - SRCS - #qshell_main.cpp - qshell_start_posix.cpp - qshell.cpp - DEPENDS - ) - diff --git a/src/drivers/qshell/posix/qshell.cpp b/src/drivers/qshell/posix/qshell.cpp deleted file mode 100644 index 6ac9b04266..0000000000 --- a/src/drivers/qshell/posix/qshell.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell.cpp - * Send shell commands to qurt - * - * @author Nicolas de Palezieux - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "qshell.h" - -px4::AppState QShell::appState; - -int QShell::main(std::vector argList) -{ - int ret = _send_cmd(argList); - - if (ret != 0) { - PX4_ERR("Could not send command"); - return -1; - } - - ret = _wait_for_retval(); - - if (ret != 0) { - PX4_ERR("Could not get return value"); - return -1; - } - - return 0; -} - -int QShell::_send_cmd(std::vector &argList) -{ - // Let's use a sequence number to check if a return value belongs to the - // command that we just sent and not a previous one that we assumed that - // it had timed out. - ++_current_sequence; - - qshell_req_s qshell_req{}; - std::string cmd; - - for (size_t i = 0; i < argList.size(); i++) { - cmd += argList[i]; - - if (i < argList.size() - 1) { - cmd += " "; - } - } - - if (cmd.size() >= qshell_req.MAX_STRLEN) { - PX4_ERR("Command too long: %d >= %d", (int) cmd.size(), (int) qshell_req.MAX_STRLEN); - return -1; - } - - PX4_INFO("Send cmd: '%s'", cmd.c_str()); - - qshell_req.strlen = cmd.size(); - strcpy((char *)qshell_req.cmd, cmd.c_str()); - qshell_req.request_sequence = _current_sequence; - - qshell_req.timestamp = hrt_absolute_time(); - _qshell_req_pub.publish(qshell_req); - - return 0; -} - -int QShell::_wait_for_retval() -{ - const hrt_abstime time_started_us = hrt_absolute_time(); - - while (hrt_elapsed_time(&time_started_us) < 3000000) { - qshell_retval_s retval; - - if (_qshell_retval_sub.update(&retval)) { - if (retval.return_sequence != _current_sequence) { - PX4_WARN("Ignoring return value with wrong sequence"); - - } else { - if (retval.return_value) { - PX4_WARN("cmd returned with: %d", retval.return_value); - } - - return 0; - } - } - - px4_usleep(1000); - } - - PX4_ERR("command timed out"); - return -1; -} diff --git a/src/drivers/qshell/posix/qshell.h b/src/drivers/qshell/posix/qshell.h deleted file mode 100644 index 8aa8cd9e54..0000000000 --- a/src/drivers/qshell/posix/qshell.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell.h - * Send shell commands to qurt - * - * @author Nicolas de Palezieux - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -class QShell -{ -public: - QShell() = default; - ~QShell() = default; - - int main(std::vector argList); - - static px4::AppState appState; /* track requests to terminate app */ - -private: - int _send_cmd(std::vector &argList); - int _wait_for_retval(); - - uORB::Publication _qshell_req_pub{ORB_ID(qshell_req)}; - - uORB::Subscription _qshell_retval_sub{ORB_ID(qshell_retval)}; - - uint32_t _current_sequence{0}; -}; diff --git a/src/drivers/qshell/posix/qshell_start_posix.cpp b/src/drivers/qshell/posix/qshell_start_posix.cpp deleted file mode 100644 index e8b681c0b6..0000000000 --- a/src/drivers/qshell/posix/qshell_start_posix.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell_start_posix.cpp - * Send shell commands to qurt - * - * @author Nicolas de Palezieux - */ - -#include "qshell.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//using namespace px4; - -extern "C" __EXPORT int qshell_main(int argc, char *argv[]); - -static void usage() -{ - PX4_DEBUG("usage: qshell cmd [args]"); -} - -int qshell_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - std::vector argList; - - for (int i = 1; i < argc; i++) { - argList.push_back(argv[i]); - } - - - QShell qshell; - return qshell.main(argList); -} diff --git a/src/drivers/qshell/qurt/CMakeLists.txt b/src/drivers/qshell/qurt/CMakeLists.txt deleted file mode 100644 index 1133de1361..0000000000 --- a/src/drivers/qshell/qurt/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE drivers__qshell__qurt - MAIN qshell - SRCS - qshell_main.cpp - qshell_start_qurt.cpp - qshell.cpp - DEPENDS - ) diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp deleted file mode 100644 index 9cd28c64fc..0000000000 --- a/src/drivers/qshell/qurt/qshell.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell.cpp - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#include "qshell.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#define MAX_ARGS 8 // max number of whitespace separated args after app name - -px4::AppState QShell::appState; - -QShell::QShell() -{ - init_app_map(m_apps); -} - -int QShell::main() -{ - appState.setRunning(true); - int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req)); - - if (sub_qshell_req == PX4_ERROR) { - PX4_ERR("Error subscribing to qshell_req topic"); - return -1; - } - - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = sub_qshell_req; - fds[0].events = POLLIN; - - while (!appState.exitRequested()) { - - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - - if (pret > 0 && fds[0].revents & POLLIN) { - - orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req); - - PX4_INFO("qshell gotten: %s", m_qshell_req.cmd); - char current_char; - std::string arg; - std::vector appargs; - - for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) { - current_char = m_qshell_req.cmd[str_idx]; - - if (isspace(current_char)) { // split at spaces - if (arg.length()) { - appargs.push_back(arg); - arg = ""; - } - - } else { - arg += current_char; - } - } - - appargs.push_back(arg); // push last argument - - qshell_retval_s retval{}; - retval.return_value = run_cmd(appargs); - retval.return_sequence = m_qshell_req.request_sequence; - - if (retval.return_value) { - PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd); - - } else { - PX4_INFO("Ok executing command: %s", m_qshell_req.cmd); - } - - retval.timestamp = hrt_absolute_time(); - _qshell_retval_pub.publish(retval); - - } else if (pret == 0) { - // Timing out is fine. - } else { - // Something is wrong. - usleep(10000); - } - } - - appState.setRunning(false); - return 0; -} - -int QShell::run_cmd(const std::vector &appargs) -{ - // command is appargs[0] - std::string command = appargs[0]; - - if (command.compare("help") == 0) { - list_builtins(m_apps); - return 0; - } - - //replaces app.find with iterator code to avoid null pointer exception - for (apps_map_type::iterator it = m_apps.begin(); it != m_apps.end(); ++it) { - if (it->first == command) { - // one for command name, one for null terminator - const char *arg[MAX_ARGS + 2]; - - unsigned int i = 0; - - if (appargs.size() > MAX_ARGS + 1) { - PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1)); - return 1; - } - - while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { - arg[i] = (char *)appargs[i].c_str(); - PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]); - ++i; - } - - arg[i] = (char *)0; - - //PX4_DEBUG_PRINTF(i); - if (m_apps[command] == NULL) { - PX4_ERR("Null function !!\n"); - - } else { - return m_apps[command](i, (char **)arg); - } - - } - } - - PX4_ERR("Command %s not found", command.c_str()); - return 1; -} diff --git a/src/drivers/qshell/qurt/qshell.h b/src/drivers/qshell/qurt/qshell.h deleted file mode 100644 index aa5ee41d6f..0000000000 --- a/src/drivers/qshell/qurt/qshell.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell.h - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "apps.h" - -class QShell -{ -public: - QShell(); - ~QShell() {} - - int main(); - int run_cmd(const std::vector &appargs); - - static px4::AppState appState; /* track requests to terminate app */ - -private: - - uORB::Publication _qshell_retval_pub{ORB_ID(qshell_retval)}; - - qshell_req_s m_qshell_req{}; - - apps_map_type m_apps; -}; diff --git a/src/drivers/qshell/qurt/qshell_main.cpp b/src/drivers/qshell/qurt/qshell_main.cpp deleted file mode 100644 index 56010acf88..0000000000 --- a/src/drivers/qshell/qurt/qshell_main.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell_main.cpp - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#include -#include -#include -#include "qshell.h" - -extern "C" __EXPORT int qshell_entry(int argc, char **argv); - -int qshell_entry(int argc, char **argv) -{ - px4::init(argc, argv, "qshell"); - - PX4_DEBUG("qshell"); - QShell qshell; - qshell.main(); - - PX4_DEBUG("goodbye"); - return 0; -} diff --git a/src/drivers/qshell/qurt/qshell_start_qurt.cpp b/src/drivers/qshell/qurt/qshell_start_qurt.cpp deleted file mode 100644 index 18abd360c6..0000000000 --- a/src/drivers/qshell/qurt/qshell_start_qurt.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file qshell_start_qurt.cpp - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#include "qshell.h" -#include -#include -#include -#include -#include -#include - -static int daemon_task; /* Handle of deamon task / thread */ - -//using namespace px4; - -extern "C" __EXPORT int qshell_main(int argc, char *argv[]); - -int qshell_entry(int argc, char **argv) -{ - //px4::init(argc, argv, "qshell"); - - PX4_DEBUG("qshell entry....."); - QShell qshell; - qshell.main(); - - PX4_DEBUG("goodbye"); - return 0; -} - -static void usage() -{ - PX4_INFO("usage: qshell {start|stop|status}"); -} - -int qshell_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (QShell::appState.isRunning()) { - PX4_INFO("already running"); - /* this is not an error */ - return 0; - } - - PX4_DEBUG("before starting the qshell_entry task"); - - daemon_task = px4_task_spawn_cmd("qshell", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 8192, - qshell_entry, - (char *const *)argv); - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - QShell::appState.requestExit(); - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (QShell::appState.isRunning()) { - PX4_INFO("is running"); - - } else { - PX4_INFO("not started"); - } - - return 0; - } - - usage(); - return 1; -} diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index 651a0e97ae..4b0255f869 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/src/lib/drivers/device/CMakeLists.txt @@ -40,12 +40,7 @@ if (${PX4_PLATFORM} STREQUAL "nuttx") if ("${CONFIG_SPI}" STREQUAL "y") list(APPEND SRCS_PLATFORM nuttx/SPI.cpp) endif() -elseif((${PX4_PLATFORM} MATCHES "qurt")) - list(APPEND SRCS_PLATFORM - qurt/I2C.cpp - qurt/SPI.cpp - ) -elseif(UNIX AND NOT APPLE AND NOT (${PX4_PLATFORM} MATCHES "qurt")) #TODO: add linux PX4 platform type +elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type # Linux I2Cdev and SPIdev list(APPEND SRCS_PLATFORM posix/I2C.cpp diff --git a/src/lib/drivers/device/i2c.h b/src/lib/drivers/device/i2c.h index bcf8ac7e21..d1d2a10f98 100644 --- a/src/lib/drivers/device/i2c.h +++ b/src/lib/drivers/device/i2c.h @@ -34,8 +34,6 @@ #ifdef __PX4_NUTTX #include "nuttx/I2C.hpp" -#elif __PX4_QURT -#include "qurt/I2C.hpp" #else #include "posix/I2C.hpp" #endif diff --git a/src/lib/drivers/device/qurt/I2C.cpp b/src/lib/drivers/device/qurt/I2C.cpp deleted file mode 100644 index 28655a926f..0000000000 --- a/src/lib/drivers/device/qurt/I2C.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file I2C.cpp - * - * Base class for devices attached via the I2C bus. - * - * @todo Bus frequency changes; currently we do nothing with the value - * that is supplied. Should we just depend on the bus knowing? - */ - -#include "I2C.hpp" - -#include - -#include - -namespace device -{ - -I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) : - CDev(name, nullptr), - _frequency(frequency) -{ - // fill in _device_id fields for a I2C device - _device_id.devid_s.devtype = device_type; - _device_id.devid_s.bus_type = DeviceBusType_I2C; - _device_id.devid_s.bus = bus; - _device_id.devid_s.address = address; -} - -I2C::I2C(const I2CSPIDriverConfig &config) - : I2C(config.devid_driver_index, config.module_name, config.bus, config.i2c_address, config.bus_frequency) -{ -} - -I2C::~I2C() -{ - if (_fd >= 0) { - ::close(_fd); - _fd = -1; - } -} - -int -I2C::init() -{ - int ret = PX4_ERROR; - - // Open the actual I2C device - char dev_path[16] {}; - snprintf(dev_path, sizeof(dev_path), DEV_FS_I2C_DEVICE_TYPE_STRING"%i", get_device_bus()); - _fd = ::open(dev_path, O_RDWR); - - if (_fd < 0) { - DEVICE_DEBUG("failed to init I2C"); - ret = -ENOENT; - goto out; - } - - // call the probe function to check whether the device is present - ret = probe(); - - if (ret != OK) { - DEVICE_DEBUG("probe failed"); - goto out; - } - - // do base class init, which will create device node, etc - ret = CDev::init(); - - if (ret != OK) { - DEVICE_DEBUG("cdev init failed"); - goto out; - } - - // tell the world where we are - DEVICE_DEBUG("on I2C bus %d at 0x%02x", get_device_bus(), get_device_address()); - -out: - - if ((ret != OK) && !(_fd < 0)) { - ::close(_fd); - _fd = -1; - } - - return ret; -} - -int -I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len) -{ - int ret = PX4_ERROR; - unsigned retry_count = 0; - - if (_fd < 0) { - PX4_ERR("I2C device not opened"); - return PX4_ERROR; - } - - do { - DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - - dspal_i2c_ioctl_slave_config slave_config{}; - slave_config.slave_address = get_device_address(); - slave_config.bus_frequency_in_khz = _frequency / 1000; - slave_config.byte_transer_timeout_in_usecs = 10000; // 10 ms - int ret_config = ::ioctl(_fd, I2C_IOCTL_SLAVE, &slave_config); - - if (ret_config < 0) { - DEVICE_DEBUG("Could not set slave config, result: %d", ret_config); - } - - - dspal_i2c_ioctl_combined_write_read ioctl_write_read{}; - - ioctl_write_read.write_buf = (uint8_t *)send; - ioctl_write_read.write_buf_len = send_len; - ioctl_write_read.read_buf = recv; - ioctl_write_read.read_buf_len = recv_len; - - int bytes_read = ::ioctl(_fd, I2C_IOCTL_RDWR, &ioctl_write_read); - - if (bytes_read != (int)recv_len) { - DEVICE_DEBUG("I2C transfer failed, bytes read %d", bytes_read); - ret = PX4_ERROR; - - } else { - // success - ret = PX4_OK; - break; - } - - } while (retry_count++ < _retries); - - return ret; -} - -} // namespace device diff --git a/src/lib/drivers/device/qurt/I2C.hpp b/src/lib/drivers/device/qurt/I2C.hpp deleted file mode 100644 index ce7bd94f83..0000000000 --- a/src/lib/drivers/device/qurt/I2C.hpp +++ /dev/null @@ -1,115 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2016-2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file I2C.hpp - * - * Base class for devices connected via I2C. - */ - -#ifndef _DEVICE_I2C_H -#define _DEVICE_I2C_H - -#include "../CDev.hpp" - -struct I2CSPIDriverConfig; - -namespace device __EXPORT -{ - -/** - * Abstract class for character device on I2C - */ -class __EXPORT I2C : public CDev -{ - -public: - - // no copy, assignment, move, move assignment - I2C(const I2C &) = delete; - I2C &operator=(const I2C &) = delete; - I2C(I2C &&) = delete; - I2C &operator=(I2C &&) = delete; - - virtual int init() override; - -protected: - /** - * The number of times a read or write operation will be retried on - * error. - */ - uint8_t _retries{0}; - - /** - * @ Constructor - * - * @param device_type The device type (see drv_sensor.h) - * @param name Driver name - * @param bus I2C bus on which the device lives - * @param address I2C bus address, or zero if set_address will be used - * @param frequency I2C bus frequency for the device (currently not used) - */ - I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency); - I2C(const I2CSPIDriverConfig &config); - virtual ~I2C(); - - /** - * Check for the presence of the device on the bus. - */ - virtual int probe() { return PX4_OK; } - - /** - * Perform an I2C transaction to the device. - * - * At least one of send_len and recv_len must be non-zero. - * - * @param send Pointer to bytes to send. - * @param send_len Number of bytes to send. - * @param recv Pointer to buffer for bytes received. - * @param recv_len Number of bytes to receive. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); - - virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } - -private: - uint32_t _frequency{0}; - int _fd{-1}; - -}; - -} // namespace device - -#endif /* _DEVICE_I2C_H */ diff --git a/src/lib/drivers/device/qurt/SPI.cpp b/src/lib/drivers/device/qurt/SPI.cpp deleted file mode 100644 index dec76b6fc9..0000000000 --- a/src/lib/drivers/device/qurt/SPI.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file SPI.cpp - * - * Base class for devices connected via SPI. - * - */ - -#include "SPI.hpp" - -#include -#include -#include - -#include "dev_fs_lib_spi.h" - -#include -#include - -namespace device -{ - -SPI::SPI(uint8_t device_type, const char *name, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency) : - CDev(name, nullptr), - _device(device), - _mode(mode), - _frequency(frequency) -{ - _device_id.devid_s.devtype = device_type; - // fill in _device_id fields for a SPI device - _device_id.devid_s.bus_type = DeviceBusType_SPI; - _device_id.devid_s.bus = bus; - _device_id.devid_s.address = (uint8_t)device; -} - -SPI::SPI(const I2CSPIDriverConfig &config) - : SPI(config.devid_driver_index, config.module_name, config.bus, config.spi_devid, config.spi_mode, - config.bus_frequency) -{ -} - -SPI::~SPI() -{ - if (_fd >= 0) { - ::close(_fd); - _fd = -1; - } -} - -int -SPI::init() -{ - // Open the actual SPI device - char dev_path[16]; - snprintf(dev_path, sizeof(dev_path), DEV_FS_SPI_DEVICE_TYPE_STRING"%lu", PX4_SPI_DEV_ID(_device)); - DEVICE_DEBUG("%s", dev_path); - _fd = ::open(dev_path, O_RDWR); - - if (_fd < 0) { - PX4_ERR("could not open %s", dev_path); - return PX4_ERROR; - } - - /* call the probe function to check whether the device is present */ - int ret = probe(); - - if (ret != OK) { - DEVICE_DEBUG("probe failed"); - return ret; - } - - /* do base class init, which will create the device node, etc. */ - ret = CDev::init(); - - if (ret != OK) { - DEVICE_DEBUG("cdev init failed"); - return ret; - } - - /* tell the world where we are */ - DEVICE_DEBUG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000); - - return PX4_OK; -} - -int -SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) -{ - if ((send == nullptr) && (recv == nullptr)) { - return -EINVAL; - } - - // set bus frequency - dspal_spi_ioctl_set_bus_frequency bus_freq{}; - - bus_freq.bus_frequency_in_hz = _frequency; - - if (::ioctl(_fd, SPI_IOCTL_SET_BUS_FREQUENCY_IN_HZ, &bus_freq) < 0) { - PX4_ERR("setting bus frequency failed"); - return PX4_ERROR; - } - - // set bus mode - // dspal_spi_ioctl_set_spi_mode bus_mode{}; - // bus_mode.eClockPolarity = SPI_CLOCK_IDLE_HIGH; - // bus_mode.eShiftMode = SPI_OUTPUT_FIRST; - - // if (::ioctl(spi_fildes, SPI_IOCTL_SET_SPI_MODE, &bus_mode) < 0) { - // PX4_ERR("setting mode failed"); - // return PX4_ERROR; - // } - - // transfer data - dspal_spi_ioctl_read_write ioctl_write_read{}; - - ioctl_write_read.read_buffer = send; - ioctl_write_read.read_buffer_length = len; - ioctl_write_read.write_buffer = recv; - ioctl_write_read.write_buffer_length = len; - - int result = ::ioctl(_fd, SPI_IOCTL_RDWR, &ioctl_write_read); - - if (result < 0) { - PX4_ERR("transfer error %d", result); - return PX4_ERROR; - } - - return PX4_OK; -} - -int -SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) -{ - if ((send == nullptr) && (recv == nullptr)) { - return -EINVAL; - } - - // set bus frequency - dspal_spi_ioctl_set_bus_frequency bus_freq{}; - bus_freq.bus_frequency_in_hz = _frequency; - - if (::ioctl(_fd, SPI_IOCTL_SET_BUS_FREQUENCY_IN_HZ, &bus_freq) < 0) { - PX4_ERR("setting bus frequency failed"); - return PX4_ERROR; - } - - // set bus mode - // dspal_spi_ioctl_set_spi_mode bus_mode{}; - // bus_mode.eClockPolarity = SPI_CLOCK_IDLE_HIGH; - // bus_mode.eShiftMode = SPI_OUTPUT_FIRST; - - // if (::ioctl(spi_fildes, SPI_IOCTL_SET_SPI_MODE, &bus_mode) < 0) { - // PX4_ERR("setting mode failed"); - // return PX4_ERROR; - // } - - // transfer data - dspal_spi_ioctl_read_write ioctl_write_read{}; - - ioctl_write_read.read_buffer = send; - ioctl_write_read.read_buffer_length = len * 2; - ioctl_write_read.write_buffer = recv; - ioctl_write_read.write_buffer_length = len * 2; - - int result = ::ioctl(_fd, SPI_IOCTL_RDWR, &ioctl_write_read); - - if (result < 0) { - PX4_ERR("transfer error %d", result); - return PX4_ERROR; - } - - return PX4_OK; -} - -} // namespace device diff --git a/src/lib/drivers/device/qurt/SPI.hpp b/src/lib/drivers/device/qurt/SPI.hpp deleted file mode 100644 index 76a7364c1d..0000000000 --- a/src/lib/drivers/device/qurt/SPI.hpp +++ /dev/null @@ -1,171 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file SPI.hpp - * - * Base class for devices connected via SPI. - */ - -#pragma once - -#include "../CDev.hpp" - -#include "dev_fs_lib_spi.h" - -enum spi_mode_e { - SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ - SPIDEV_MODE1 = 1, /* CPOL=0 CHPHA=1 */ - SPIDEV_MODE2 = 2, /* CPOL=1 CHPHA=0 */ - SPIDEV_MODE3 = 3 /* CPOL=1 CHPHA=1 */ -}; - -struct I2CSPIDriverConfig; - -namespace device __EXPORT -{ - -/** - * Abstract class for character device on SPI - */ -class __EXPORT SPI : public CDev -{ -protected: - /** - * Constructor - * - * @param device_type The device type (see drv_sensor.h) - * @param name Driver name - * @param bus SPI bus on which the device lives - * @param device Device handle (used by SPI_SELECT) - * @param mode SPI clock/data mode - * @param frequency SPI clock frequency - */ - SPI(uint8_t device_type, const char *name, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency); - SPI(const I2CSPIDriverConfig &config); - 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(); - - /** - * Check for the presence of the device on the bus. - */ - virtual int probe() { return PX4_OK; } - - /** - * Perform a SPI transfer. - * - * If called from interrupt context, this interface does not lock - * the bus and may interfere with non-interrupt-context callers. - * - * Clients in a mixed interrupt/non-interrupt configuration must - * ensure appropriate interlocking. - * - * At least one of send or recv must be non-null. - * - * @param send Bytes to send to the device, or nullptr if - * no data is to be sent. - * @param recv Buffer for receiving bytes from the device, - * or nullptr if no bytes are to be received. - * @param len Number of bytes to transfer. - * @return OK if the exchange was successful, -errno - * otherwise. - */ - int transfer(uint8_t *send, uint8_t *recv, unsigned len); - - /** - * Perform a SPI 16 bit transfer. - * - * If called from interrupt context, this interface does not lock - * the bus and may interfere with non-interrupt-context callers. - * - * Clients in a mixed interrupt/non-interrupt configuration must - * ensure appropriate interlocking. - * - * At least one of send or recv must be non-null. - * - * @param send Words to send to the device, or nullptr if - * no data is to be sent. - * @param recv Words for receiving bytes from the device, - * or nullptr if no bytes are to be received. - * @param len Number of words to transfer. - * @return OK if the exchange was successful, -errno - * otherwise. - */ - int transferhword(uint16_t *send, uint16_t *recv, unsigned len); - - /** - * Set the SPI bus frequency - * This is used to change frequency on the fly. Some sensors - * (such as the MPU6000) need a lower frequency for setup - * registers and can handle higher frequency for sensor - * value registers - * - * @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) {} - -private: - int _fd{-1}; - - uint32_t _device; - enum spi_mode_e _mode; - uint32_t _frequency; - -protected: - - bool external() { return px4_spi_bus_external(get_device_bus()); } - -}; - -} // namespace device diff --git a/src/lib/drivers/device/spi.h b/src/lib/drivers/device/spi.h index 5a3f4d4d37..9ca739d180 100644 --- a/src/lib/drivers/device/spi.h +++ b/src/lib/drivers/device/spi.h @@ -34,8 +34,6 @@ #ifdef __PX4_NUTTX #include "nuttx/SPI.hpp" -#elif __PX4_QURT -#include "qurt/SPI.hpp" #else #include "posix/SPI.hpp" #endif diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index f6a1461f0c..77d1991f53 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -100,14 +100,9 @@ bool __EXPORT less_than_or_equal(float a, float b) } } -void __EXPORT float2SigExp( - const float &num, - float &sig, - int &exp) +void __EXPORT float2SigExp(const float &num, float &sig, int &exp) { -// FIXME - This code makes no sense when exp is an int -// FIXME - isnan and isinf not defined for QuRT -#ifndef __PX4_QURT + if (!PX4_ISFINITE(num)) { sig = 0.0f; exp = -99; @@ -129,8 +124,6 @@ void __EXPORT float2SigExp( exp = floor(exp); } -#endif - sig = num; // cheap power since it is integer diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index 31945283fe..64bde57971 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -129,17 +129,12 @@ add_custom_command(OUTPUT px4_parameters.hpp ) set(SRCS) -if ("${CONFIG_SHMEM}" STREQUAL "1") - message(STATUS "parameters shared memory enabled") - add_definitions(-DCONFIG_SHMEM=1) - list(APPEND SRCS parameters_shmem.cpp) + +list(APPEND SRCS parameters.cpp) +if(BUILD_TESTING) + list(APPEND SRCS param_translation_unit_tests.cpp) else() - list(APPEND SRCS parameters.cpp) - if(BUILD_TESTING) - list(APPEND SRCS param_translation_unit_tests.cpp) - else() - list(APPEND SRCS param_translation.cpp) - endif() + list(APPEND SRCS param_translation.cpp) endif() if(${PX4_PLATFORM} STREQUAL "nuttx") diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 328f16ed8a..38a170ece8 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -81,14 +81,6 @@ static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; static char *param_user_file = nullptr; -#ifdef __PX4_QURT -#define PARAM_OPEN px4_open -#define PARAM_CLOSE px4_close -#else -#define PARAM_OPEN open -#define PARAM_CLOSE close -#endif - #include /* autosaving variables */ static hrt_abstime last_autosave_timestamp = 0; @@ -1133,11 +1125,11 @@ int param_save_default() while (res != OK && attempts > 0) { // write parameters to file - int fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); + int fd = ::open(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); if (fd > -1) { res = param_export(fd, false, nullptr); - PARAM_CLOSE(fd); + ::close(fd); if (res != PX4_OK) { PX4_ERR("param_export failed, retrying %d", attempts); @@ -1170,7 +1162,7 @@ param_load_default() return flash_param_load(); } - int fd_load = PARAM_OPEN(filename, O_RDONLY); + int fd_load = ::open(filename, O_RDONLY); if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ @@ -1183,7 +1175,7 @@ param_load_default() } int result = param_load(fd_load); - PARAM_CLOSE(fd_load); + ::close(fd_load); if (result != 0) { PX4_ERR("error reading parameters from '%s'", filename); diff --git a/src/lib/parameters/parameters_shmem.cpp b/src/lib/parameters/parameters_shmem.cpp deleted file mode 100644 index 415bfd3191..0000000000 --- a/src/lib/parameters/parameters_shmem.cpp +++ /dev/null @@ -1,1423 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file param.c - * - * Global parameter store. - * - * Note that it might make sense to convert this into a driver. That would - * offer some interesting options regarding state for e.g. ORB advertisements - * and background parameter saving. - */ - -#include "param.h" -#include -#include "tinybson/tinybson.h" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include "uthash/utarray.h" - -#include "uORB/uORB.h" -#include "uORB/topics/parameter_update.h" - -#if defined(FLASH_BASED_PARAMS) -#include "flashparams/flashparams.h" -#endif - -#include - -#include - -#ifdef __PX4_QURT -static const char *param_default_file = "/dev/fs/params"; -#else -static const char *param_default_file = "/usr/share/data/adsp/params"; -#endif -static char *param_user_file = nullptr; - -#ifdef __PX4_QURT -//Mode not supported by qurt -#define PARAM_OPEN(a, b, ...) open(a, b) -#else -#define PARAM_OPEN open -#endif -#define PARAM_CLOSE close - -#include -/* autosaving variables */ -static hrt_abstime last_autosave_timestamp = 0; -static struct work_s autosave_work; -static bool autosave_scheduled = false; -static bool autosave_disabled = false; - -/** - * Array of static parameter info. - */ -static const param_info_s *param_info_base = (const param_info_s *) &px4_parameters; -#define param_info_count px4_parameters.param_count - -/** - * Storage for modified parameters. - */ -struct param_wbuf_s { - union param_value_u val; - param_t param; - bool unsaved; -}; - - -uint8_t *param_changed_storage = nullptr; -int size_param_changed_storage_bytes = 0; -const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); - -//#define ENABLE_SHMEM_DEBUG -static void init_params(); - -static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes); -static unsigned char set_called_from_get = 0; - -static int param_import_done = - 0; /*at startup, params are loaded from file, if present. we dont want to send notifications that time since muorb is not ready*/ - -static int param_load_default_no_notify(); - -static unsigned -get_param_info_count() -{ - /* Singleton creation of and array of bits to track changed values */ - if (!param_changed_storage) { - /* Note that we have a (highly unlikely) race condition here: in the worst case the allocation is done twice */ - size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit) + 1; - param_changed_storage = (uint8_t *)calloc(size_param_changed_storage_bytes, 1); - - /* If the allocation fails we need to indicate failure in the - * API by returning PARAM_INVALID - */ - if (param_changed_storage == nullptr) { - return 0; - } - } - - return param_info_count; -} - -/** flexible array holding modified parameter values */ -UT_array *param_values{nullptr}; - -/** array info for the modified parameters array */ -const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr}; - -static orb_advert_t param_topic = nullptr; -static unsigned int param_instance = 0; - -static void param_set_used_internal(param_t param); - -static param_t param_find_internal(const char *name, bool notification); - -// TODO: not working on Snappy just yet -// the following implements an RW-lock using 2 semaphores (used as mutexes). It gives -// priority to readers, meaning a writer could suffer from starvation, but in our use-case -// we only have short periods of reads and writes are rare. -//static px4_sem_t param_sem; ///< this protects against concurrent access to param_values -//static int reader_lock_holders = 0; -//static px4_sem_t reader_lock_holders_lock; ///< this protects against concurrent access to reader_lock_holders - -static perf_counter_t param_export_perf; -static perf_counter_t param_find_perf; -static perf_counter_t param_get_perf; -static perf_counter_t param_set_perf; - -//static px4_sem_t param_sem_save; ///< this protects against concurrent param saves (file or flash access). -///< we use a separate lock to allow concurrent param reads and saves. -///< a param_set could still be blocked by a param save, because it -///< needs to take the reader lock - -/** lock the parameter store for read access */ -static void -param_lock_reader() -{ - // TODO: this doesn't seem to work on Snappy -#if 0 - do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0); - - ++reader_lock_holders; - - if (reader_lock_holders == 1) { - // the first reader takes the lock, the next ones are allowed to just continue - do {} while (px4_sem_wait(¶m_sem) != 0); - } - - px4_sem_post(&reader_lock_holders_lock); -#endif -} - -/** lock the parameter store for write access */ -static void -param_lock_writer() -{ - // TODO: this doesn't seem to work on Snappy -#if 0 - do {} while (px4_sem_wait(¶m_sem) != 0); - -#endif -} - -/** unlock the parameter store */ -static void -param_unlock_reader() -{ - // TODO: this doesn't seem to work on Snappy -#if 0 - do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0); - - --reader_lock_holders; - - if (reader_lock_holders == 0) { - // the last reader releases the lock - px4_sem_post(¶m_sem); - } - - px4_sem_post(&reader_lock_holders_lock); -#endif -} - -/** unlock the parameter store */ -static void -param_unlock_writer() -{ - // TODO: this doesn't seem to work on Snappy -#if 0 - px4_sem_post(¶m_sem); -#endif -} - -/** assert that the parameter store is locked */ -static void -param_assert_locked() -{ - /* XXX */ -} - -void -param_init() -{ - // TODO: not needed on Snappy yet. - //px4_sem_init(¶m_sem, 0, 1); - //px4_sem_init(¶m_sem_save, 0, 1); - //px4_sem_init(&reader_lock_holders_lock, 0, 1); - - param_export_perf = perf_alloc(PC_ELAPSED, "param_export"); - param_find_perf = perf_alloc(PC_ELAPSED, "param_find"); - param_get_perf = perf_alloc(PC_ELAPSED, "param_get"); - param_set_perf = perf_alloc(PC_ELAPSED, "param_set"); - -#ifdef CONFIG_SHMEM - PX4_DEBUG("Syncing params to shared memory\n"); - init_params(); -#endif -} - -/** - * Test whether a param_t is value. - * - * @param param The parameter handle to test. - * @return True if the handle is valid. - */ -bool -handle_in_range(param_t param) -{ - unsigned count = get_param_info_count(); - return (count && param < count); -} - -/** - * Compare two modifid parameter structures to determine ordering. - * - * This function is suitable for passing to qsort or bsearch. - */ -static int -param_compare_values(const void *a, const void *b) -{ - struct param_wbuf_s *pa = (struct param_wbuf_s *)a; - struct param_wbuf_s *pb = (struct param_wbuf_s *)b; - - if (pa->param < pb->param) { - return -1; - } - - if (pa->param > pb->param) { - return 1; - } - - return 0; -} - -/** - * Locate the modified parameter structure for a parameter, if it exists. - * - * @param param The parameter being searched. - * @return The structure holding the modified value, or - * nullptr if the parameter has not been modified. - */ -param_wbuf_s * -param_find_changed(param_t param) -{ - param_wbuf_s *s = nullptr; - - param_assert_locked(); - - if (param_values != nullptr) { - param_wbuf_s key{}; - key.param = param; - s = (param_wbuf_s *)utarray_find(param_values, &key, param_compare_values); - } - - return s; -} - -static void -_param_notify_changes() -{ - parameter_update_s pup = {}; - pup.timestamp = hrt_absolute_time(); - pup.instance = param_instance++; - - /* - * If we don't have a handle to our topic, create one now; otherwise - * just publish. - */ - if (param_topic == nullptr) { - param_topic = orb_advertise(ORB_ID(parameter_update), &pup); - - } else { - orb_publish(ORB_ID(parameter_update), param_topic, &pup); - } -} - -void -param_notify_changes() -{ - _param_notify_changes(); -} - -param_t -param_find_internal(const char *name, bool notification) -{ - perf_begin(param_find_perf); - - param_t param; - - /* perform a linear search of the known parameters */ - for (param = 0; handle_in_range(param); param++) { - if (!strcmp(param_info_base[param].name, name)) { - if (notification) { - param_set_used_internal(param); - } - - perf_end(param_find_perf); - return param; - } - } - - perf_end(param_find_perf); - - /* not found */ - return PARAM_INVALID; -} - -param_t -param_find(const char *name) -{ - return param_find_internal(name, true); -} - -param_t -param_find_no_notification(const char *name) -{ - return param_find_internal(name, false); -} - -unsigned -param_count() -{ - return get_param_info_count(); -} - -unsigned -param_count_used() -{ - //TODO FIXME: all params used right now -#if 0 - unsigned count = 0; - - // ensure the allocation has been done - if (get_param_info_count()) { - - for (int i = 0; i < size_param_changed_storage_bytes; i++) { - for (int j = 0; j < bits_per_allocation_unit; j++) { - if (param_changed_storage[i] & (1 << j)) { - count++; - } - } - } - } - - return count; -#else - return get_param_info_count(); -#endif -} - -param_t -param_for_index(unsigned index) -{ - unsigned count = get_param_info_count(); - - if (count && index < count) { - return (param_t)index; - } - - return PARAM_INVALID; -} - -param_t -param_for_used_index(unsigned index) -{ -#if 0 - int count = get_param_info_count(); - - if (count && (int)index < count) { - /* walk all params and count used params */ - unsigned used_count = 0; - - for (int i = 0; i < size_param_changed_storage_bytes; i++) { - for (int j = 0; j < bits_per_allocation_unit; j++) { - if (param_changed_storage[i] & (1 << j)) { - - /* we found the right used count, - * return the param value - */ - if (index == used_count) { - return (param_t)(i * bits_per_allocation_unit + j); - } - - used_count++; - } - } - } - } - - return PARAM_INVALID; -#else - return param_for_index(index); -#endif -} - -int -param_get_index(param_t param) -{ - if (handle_in_range(param)) { - return (unsigned)param; - } - - return -1; -} - -int -param_get_used_index(param_t param) -{ - // TODO FIXME: the used bit is not supported right now, therefore just count all. -#if 0 - /* this tests for out of bounds and does a constant time lookup */ - if (!param_used(param)) { - return -1; - } - - /* walk all params and count, now knowing that it has a valid index */ - int used_count = 0; - - for (int i = 0; i < size_param_changed_storage_bytes; i++) { - for (int j = 0; j < bits_per_allocation_unit; j++) { - if (param_changed_storage[i] & (1 << j)) { - - if ((int)param == i * bits_per_allocation_unit + j) { - return used_count; - } - - used_count++; - } - } - } - - return -1; -#else - return param; -#endif - -} - -const char * -param_name(param_t param) -{ - return handle_in_range(param) ? param_info_base[param].name : nullptr; -} - -bool -param_is_volatile(param_t param) -{ - return handle_in_range(param) ? param_info_base[param].volatile_param : false; -} - -bool -param_value_is_default(param_t param) -{ - struct param_wbuf_s *s; - param_lock_reader(); - s = param_find_changed(param); - param_unlock_reader(); - return s ? false : true; -} - -bool -param_value_unsaved(param_t param) -{ - struct param_wbuf_s *s; - param_lock_reader(); - s = param_find_changed(param); - bool ret = s && s->unsaved; - param_unlock_reader(); - return ret; -} - -param_type_t -param_type(param_t param) -{ - return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN; -} - -size_t -param_size(param_t param) -{ - if (handle_in_range(param)) { - switch (param_type(param)) { - case PARAM_TYPE_INT32: - case PARAM_TYPE_FLOAT: - return 4; - - default: - return 0; - } - } - - return 0; -} - -/** - * Obtain a pointer to the storage allocated for a parameter. - * - * @param param The parameter whose storage is sought. - * @return A pointer to the parameter value, or nullptr - * if the parameter does not exist. - */ -static const void * -param_get_value_ptr(param_t param) -{ - const void *result = nullptr; - - param_assert_locked(); - - if (handle_in_range(param)) { - - const union param_value_u *v; - - /* work out whether we're fetching the default or a written value */ - struct param_wbuf_s *s = param_find_changed(param); - - if (s != nullptr) { - v = &s->val; - - } else { - v = ¶m_info_base[param].val; - } - - result = v; - } - - return result; -} - -int -param_get(param_t param, void *val) -{ - int result = -1; - - param_lock_reader(); - perf_begin(param_get_perf); - - if (!handle_in_range(param)) { - return result; - } - - union param_value_u value; - - if (update_from_shmem(param, &value)) { - set_called_from_get = 1; - param_set_internal(param, &value, true, false); - set_called_from_get = 0; - } - - const void *v = param_get_value_ptr(param); - - if (val && v) { - memcpy(val, v, param_size(param)); - result = 0; - } - -#ifdef ENABLE_SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("param_get for %s : %d", param_name(param), ((union param_value_u *)val)->i); - } - - else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("param_get for %s : %f", param_name(param), (double)((union param_value_u *)val)->f); - } - - else { - PX4_INFO("Unknown param type for %s", param_name(param)); - } - -#endif - - perf_end(param_get_perf); - param_unlock_reader(); - - return result; -} - -/** - * worker callback method to save the parameters - * @param arg unused - */ -static void -autosave_worker(void *arg) -{ - bool disabled = false; - - param_lock_writer(); - last_autosave_timestamp = hrt_absolute_time(); - autosave_scheduled = false; - disabled = autosave_disabled; - param_unlock_writer(); - - if (disabled) { - return; - } - - PX4_DEBUG("Autosaving params"); - int ret = param_save_default(); - - if (ret != 0) { - PX4_ERR("param save failed (%i)", ret); - } -} - -/** - * Automatically save the parameters after a timeout and limited rate. - * - * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it - * needs to be the same lock as autosave_worker() and param_control_autosave() use). - */ -static void -param_autosave() -{ - if (autosave_scheduled || autosave_disabled) { - return; - } - - // wait at least 300ms before saving, because: - // - tasks often call param_set() for multiple params, so this avoids unnecessary save calls - // - the logger stores changed params. He gets notified on a param change via uORB and then - // looks at all unsaved params. - hrt_abstime delay = 300 * 1000; - - const hrt_abstime rate_limit = 2000 * 1000; // rate-limit saving to 2 seconds - hrt_abstime last_save_elapsed = hrt_elapsed_time(&last_autosave_timestamp); - - if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { - delay = rate_limit - last_save_elapsed; - } - - autosave_scheduled = true; - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay)); -} - -void -param_control_autosave(bool enable) -{ - param_lock_writer(); - - if (!enable && autosave_scheduled) { - work_cancel(LPWORK, &autosave_work); - autosave_scheduled = false; - } - - autosave_disabled = !enable; - param_unlock_writer(); -} - -static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) -{ - int result = -1; - bool params_changed = false; - - param_lock_writer(); - perf_begin(param_set_perf); - - if (param_values == nullptr) { - utarray_new(param_values, ¶m_icd); - } - - if (param_values == nullptr) { - PX4_ERR("failed to allocate modified values array"); - goto out; - } - - if (handle_in_range(param)) { - - param_wbuf_s *s = param_find_changed(param); - - if (s == nullptr) { - - /* construct a new parameter */ - param_wbuf_s buf = {}; - buf.param = param; - - params_changed = true; - - /* add it to the array and sort */ - utarray_push_back(param_values, &buf); - utarray_sort(param_values, param_compare_values); - - /* find it after sorting */ - s = param_find_changed(param); - } - - /* update the changed value */ - switch (param_type(param)) { - case PARAM_TYPE_INT32: - params_changed = params_changed || s->val.i != *(int32_t *)val; - s->val.i = *(int32_t *)val; - break; - - case PARAM_TYPE_FLOAT: - params_changed = params_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON; - s->val.f = *(float *)val; - break; - - default: - goto out; - } - - s->unsaved = !mark_saved; - result = 0; - - if (!mark_saved) { // this is false when importing parameters - param_autosave(); - } - } - -out: - perf_end(param_set_perf); - param_unlock_writer(); - - /* - * If we set something, now that we have unlocked, go ahead and advertise that - * a thing has been set. - */ - - if (!param_import_done) { notify_changes = 0; } - - if (params_changed && notify_changes) { - _param_notify_changes(); - } - - if (result == 0 && !set_called_from_get) { - update_to_shmem(param, *(union param_value_u *)val); - } - -#ifdef ENABLE_SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("param_set for %s : %d", param_name(param), ((union param_value_u *)val)->i); - } - - else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("param_set for %s : %f", param_name(param), (double)((union param_value_u *)val)->f); - } - - else { - PX4_INFO("Unknown param type for %s", param_name(param)); - } - -#endif - - return result; -} - -#if defined(FLASH_BASED_PARAMS) -int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes) -{ - return param_set_internal(param, val, mark_saved, notify_changes); -} - -const void *param_get_value_ptr_external(param_t param) -{ - return param_get_value_ptr(param); -} -#endif - -int -param_set(param_t param, const void *val) -{ - return param_set_internal(param, val, false, true); -} - -int -param_set_no_notification(param_t param, const void *val) -{ - return param_set_internal(param, val, false, false); -} - -bool -param_used(param_t param) -{ - // TODO FIXME: for now all params are used - return true; - - int param_index = param_get_index(param); - - if (param_index < 0) { - return false; - } - - return param_changed_storage[param_index / bits_per_allocation_unit] & - (1 << param_index % bits_per_allocation_unit); -} - -void param_set_used(param_t param) -{ - param_set_used_internal(param); -} - -void param_set_used_internal(param_t param) -{ - int param_index = param_get_index(param); - - if (param_index < 0) { - return; - } - - // FIXME: this needs locking too - param_changed_storage[param_index / bits_per_allocation_unit] |= - (1 << param_index % bits_per_allocation_unit); -} - -int -param_reset(param_t param) -{ - param_wbuf_s *s = nullptr; - bool param_found = false; - - param_lock_writer(); - - if (handle_in_range(param)) { - - /* look for a saved value */ - s = param_find_changed(param); - - /* if we found one, erase it */ - if (s != nullptr) { - int pos = utarray_eltidx(param_values, s); - utarray_erase(param_values, pos, 1); - } - - param_found = true; - } - - param_autosave(); - - param_unlock_writer(); - - if (s != nullptr) { - _param_notify_changes(); - } - - return (!param_found); -} -static void -param_reset_all_internal(bool auto_save) -{ - param_lock_writer(); - - if (param_values != nullptr) { - utarray_free(param_values); - } - - /* mark as reset / deleted */ - param_values = nullptr; - - if (auto_save) { - param_autosave(); - } - - param_unlock_writer(); - - _param_notify_changes(); -} - -void -param_reset_all() -{ - param_reset_all_internal(true); -} - -void -param_reset_excludes(const char *excludes[], int num_excludes) -{ - param_t param; - - for (param = 0; handle_in_range(param); param++) { - const char *name = param_name(param); - bool exclude = false; - - for (int index = 0; index < num_excludes; index ++) { - int len = strlen(excludes[index]); - - if ((excludes[index][len - 1] == '*' - && strncmp(name, excludes[index], len - 1) == 0) - || strcmp(name, excludes[index]) == 0) { - - exclude = true; - break; - } - } - - if (!exclude) { - param_reset(param); - } - } - - _param_notify_changes(); -} - -int -param_set_default_file(const char *filename) -{ - if (param_user_file != nullptr) { - // we assume this is not in use by some other thread - free(param_user_file); - param_user_file = nullptr; - } - - if (filename) { - param_user_file = strdup(filename); - } - - return 0; -} - -const char * -param_get_default_file() -{ - return (param_user_file != nullptr) ? param_user_file : param_default_file; -} - -int -param_save_default() -{ - int res = OK; - int fd = -1; - - const char *filename = param_get_default_file(); - - fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); - - if (fd < 0) { - PX4_ERR("failed to open param file: %s", filename); - goto do_exit; - } - - res = param_export(fd, false, nullptr); - - if (res != OK) { - PX4_ERR("failed to write parameters to file: %s", filename); - goto do_exit; - } - - PARAM_CLOSE(fd); - - - fd = -1; - -do_exit: - - if (fd >= 0) { - close(fd); - } - - if (res == OK) { - PX4_DEBUG("saving params completed successfully"); - } - - return res; -} - -/** - * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed - */ -int -param_load_default() -{ - int res = 0; -#if !defined(FLASH_BASED_PARAMS) - int fd_load = PARAM_OPEN(param_get_default_file(), O_RDONLY); - - if (fd_load < 0) { - /* no parameter file is OK, otherwise this is an error */ - if (errno != ENOENT) { - PX4_ERR("open '%s' for reading failed", param_get_default_file()); - return -1; - } - - return 1; - } - - int result = param_load(fd_load); - PARAM_CLOSE(fd_load); - - if (result != 0) { - PX4_ERR("error reading parameters from '%s'", param_get_default_file()); - return -2; - } - -#else - // no need for locking - res = flash_param_load(); -#endif - return res; -} - -/** - * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed - */ -static int -param_load_default_no_notify() -{ - int fd_load = open(param_get_default_file(), O_RDONLY); - - if (fd_load < 0) { -#ifdef __PX4_QURT - release_shmem_lock(__FILE__, __LINE__); -#endif - - /* no parameter file is OK, otherwise this is an error */ - if (errno != ENOENT) { - PX4_DEBUG("open '%s' for reading failed", param_get_default_file()); - return -1; - } - - return 1; - } - - int result = param_import(fd_load, true); - - close(fd_load); - - PX4_DEBUG("param loading done"); - - if (result != 0) { - PX4_WARN("error reading parameters from '%s'", param_get_default_file()); - return -2; - } - - return 0; -} - -int -param_export(int fd, bool only_unsaved, param_filter_func filter) -{ - perf_begin(param_export_perf); - - param_wbuf_s *s = nullptr; - int result = -1; - - struct bson_encoder_s encoder; - - int shutdown_lock_ret = px4_shutdown_lock(); - - if (shutdown_lock_ret) { - PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); - } - - // take the file lock - //do {} while (px4_sem_wait(¶m_sem_save) != 0); - - param_lock_reader(); - - bson_encoder_init_file(&encoder, fd); - - /* no modified parameters -> we are done */ - if (param_values == nullptr) { - result = 0; - goto out; - } - - /* First of all, update the index which will call param_get for params - * that have recently been changed. */ - update_index_from_shmem(); - - while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) { - /* - * If we are only saving values changed since last save, and this - * one hasn't, then skip it - */ - if (only_unsaved && !s->unsaved) { - continue; - } - - if (filter && !filter(s->param)) { - continue; - } - - s->unsaved = false; - - /* Make sure to get latest from shmem before saving. */ - update_from_shmem(s->param, &s->val); - - const char *name = param_name(s->param); - const size_t size = param_size(s->param); - - /* append the appropriate BSON type object */ - switch (param_type(s->param)) { - - case PARAM_TYPE_INT32: { - const int32_t i = s->val.i; - - PX4_DEBUG("exporting: %s (%d) size: %d val: %d", name, s->param, size, i); - - if (bson_encoder_append_int(&encoder, name, i)) { - PX4_ERR("BSON append failed for '%s'", name); - goto out; - } - } - break; - - case PARAM_TYPE_FLOAT: { - const double f = (double)s->val.f; - - PX4_DEBUG("exporting: %s (%d) size: %d val: %.3f", name, s->param, size, (double)f); - - if (bson_encoder_append_double(&encoder, name, f)) { - PX4_ERR("BSON append failed for '%s'", name); - goto out; - } - } - break; - - default: - PX4_ERR("unrecognized parameter type"); - goto out; - } - } - - result = 0; - -out: - param_unlock_reader(); - - //px4_sem_post(¶m_sem_save); - - fsync(fd); // make sure the data is flushed before releasing the shutdown lock - - if (shutdown_lock_ret == 0) { - px4_shutdown_unlock(); - } - - if (result == 0) { - result = bson_encoder_fini(&encoder); - } - - perf_end(param_export_perf); - - return result; -} - -struct param_import_state { - bool mark_saved; -}; - -static int -param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node) -{ - float f = 0.0f; - int32_t i = 0; - void *v = nullptr; - int result = -1; - param_import_state *state = (param_import_state *)priv; - - /* - * EOO means the end of the parameter object. (Currently not supporting - * nested BSON objects). - */ - if (node->type == BSON_EOO) { - PX4_DEBUG("end of parameters"); - return 0; - } - - /* - * Find the parameter this node represents. If we don't know it, - * ignore the node. - */ - param_t param = param_find_no_notification(node->name); - - if (param == PARAM_INVALID) { - PX4_ERR("ignoring unrecognised parameter '%s'", node->name); - return 1; - } - - /* - * Handle setting the parameter from the node - */ - - switch (node->type) { - case BSON_INT32: { - if (param_type(param) != PARAM_TYPE_INT32) { - PX4_WARN("unexpected type for %s", node->name); - result = 1; // just skip this entry - goto out; - } - - i = node->i; - v = &i; - - PX4_DEBUG("Imported %s with value %d", param_name(param), i); - } - break; - - case BSON_DOUBLE: { - if (param_type(param) != PARAM_TYPE_FLOAT) { - PX4_WARN("unexpected type for %s", node->name); - result = 1; // just skip this entry - goto out; - } - - f = node->d; - v = &f; - - PX4_DEBUG("Imported %s with value %f", param_name(param), (double)f); - } - break; - - default: - PX4_DEBUG("unrecognised node type"); - goto out; - } - - if (param_set_internal(param, v, state->mark_saved, true)) { - PX4_DEBUG("error setting value for '%s'", node->name); - goto out; - } - - /* don't return zero, that means EOF */ - result = 1; - -out: - return result; -} - -static int -param_import_internal(int fd, bool mark_saved) -{ - bson_decoder_s decoder; - param_import_state state; - int result = -1; - - if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { - PX4_ERR("decoder init failed"); - return PX4_ERROR; - } - - state.mark_saved = mark_saved; - - do { - result = bson_decoder_next(&decoder); - usleep(1); - - } while (result > 0); - - return result; -} - -int -param_import(int fd, bool mark_saved) -{ -#if !defined(FLASH_BASED_PARAMS) - return param_import_internal(fd, mark_saved); -#else - (void)fd; // unused - // no need for locking here - return flash_param_import(); -#endif -} - -int -param_load(int fd) -{ - param_reset_all_internal(false); - return param_import_internal(fd, true); -} - -void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) -{ - param_t param; - - for (param = 0; handle_in_range(param); param++) { - - /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == nullptr)) { - continue; - } - - if (only_used && !param_used(param)) { - continue; - } - - func(arg, param); - } -} - -uint32_t param_hash_check() -{ - uint32_t param_hash = 0; - - param_lock_reader(); - - /* compute the CRC32 over all string param names and 4 byte values */ - for (param_t param = 0; handle_in_range(param); param++) { - if (!param_used(param) || param_is_volatile(param)) { - continue; - } - - const char *name = param_name(param); - const void *val = param_get_value_ptr(param); - param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash); - param_hash = crc32part((const uint8_t *)val, param_size(param), param_hash); - } - - param_unlock_reader(); - - return param_hash; -} - -void param_print_status() -{ - PX4_INFO("summary: %d/%d (used/total)", param_count_used(), param_count()); - -#ifndef FLASH_BASED_PARAMS - const char *filename = param_get_default_file(); - - if (filename != nullptr) { - PX4_INFO("file: %s", param_get_default_file()); - } - -#endif /* FLASH_BASED_PARAMS */ - - if (param_values != nullptr) { - PX4_INFO("storage array: %d/%d elements (%zu bytes total)", - utarray_len(param_values), param_values->n, param_values->n * sizeof(UT_icd)); - } - - PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on"); - - if (!autosave_disabled && (last_autosave_timestamp > 0)) { - PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6); - } - - perf_print_counter(param_export_perf); - perf_print_counter(param_find_perf); - perf_print_counter(param_get_perf); - perf_print_counter(param_set_perf); -} - -void init_params() -{ -#ifdef __PX4_QURT - //copy params to shared memory - init_shared_memory(); -#endif - - /*load params automatically*/ -#ifdef __PX4_POSIX - param_load_default_no_notify(); -#endif - - param_import_done = 1; - -#ifdef __PX4_QURT - copy_params_to_shmem(param_info_base); - -#ifdef ENABLE_SHMEM_DEBUG - PX4_INFO("Offsets:"); - PX4_INFO("params_val %lu, krait_changed %lu, adsp_changed %lu", - (unsigned char *)shmem_info_p->params_val - (unsigned char *)shmem_info_p, - (unsigned char *)&shmem_info_p->krait_changed_index - (unsigned char *)shmem_info_p, - (unsigned char *)&shmem_info_p->adsp_changed_index - (unsigned char *)shmem_info_p); -#endif /* ENABLE_SHMEM_DEBUG */ - -#endif /* __PX4_QURT */ -} diff --git a/src/lib/perf/perf_counter.cpp b/src/lib/perf/perf_counter.cpp index 82b2d3a177..79eed9fc43 100644 --- a/src/lib/perf/perf_counter.cpp +++ b/src/lib/perf/perf_counter.cpp @@ -49,11 +49,6 @@ #include "perf_counter.h" -#ifdef __PX4_QURT -// There is presumably no dprintf on QURT. Therefore use the usual output to mini-dm. -#define dprintf(_fd, _text, ...) ((_fd) == 1 ? PX4_INFO((_text), ##__VA_ARGS__) : (void)(_fd)) -#endif - /** * Header common to all counters. */ diff --git a/src/modules/muorb/adsp/CMakeLists.txt b/src/modules/muorb/adsp/CMakeLists.txt deleted file mode 100644 index de899113b0..0000000000 --- a/src/modules/muorb/adsp/CMakeLists.txt +++ /dev/null @@ -1,38 +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_library(modules__muorb__adsp - px4muorb.cpp - uORBFastRpcChannel.cpp -) -target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/platforms/common/uORB) diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp deleted file mode 100644 index 095fab6903..0000000000 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ /dev/null @@ -1,295 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "px4muorb.hpp" -#include "uORBFastRpcChannel.hpp" -#include "uORBManager.hpp" - -#include -#include -#include -#include "uORB.h" -#include -#include -#include - -__BEGIN_DECLS -extern int dspal_main(int argc, char *argv[]); -__END_DECLS - -int px4muorb_orb_initialize() -{ - HAP_power_request(100, 100, 1000); - shmem_info_p = NULL; - - // The uORB Manager needs to be initialized first up, otherwise the instance is nullptr. - uORB::Manager::initialize(); - // Register the fastrpc muorb with uORBManager. - uORB::Manager::get_instance()->set_uorb_communicator( - uORB::FastRpcChannel::GetInstance()); - - // Now continue with the usual dspal startup. - const char *argv[] = { "dspal", "start" }; - int argc = 2; - int rc; - rc = dspal_main(argc, (char **) argv); - - return rc; -} - -int px4muorb_set_absolute_time_offset(int32_t time_diff_us) -{ - return hrt_set_absolute_time_offset(time_diff_us); -} - -int px4muorb_get_absolute_time(uint64_t *time_us) -{ - *time_us = hrt_absolute_time(); - return 0; -} - -/*update value and param's change bit in shared memory*/ -int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, - int data_len_in_bytes) -{ - unsigned int byte_changed, bit_changed; - union param_value_u *param_value = (union param_value_u *) value; - - if (!shmem_info_p) { - init_shared_memory(); - } - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_INFO("Could not get shmem lock\n"); - return -1; - } - - shmem_info_p->params_val[param] = *param_value; - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->krait_changed_index[byte_changed] |= bit_changed; - - release_shmem_lock(__FILE__, __LINE__); - - return 0; -} - -int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) -{ - if (!shmem_info_p) { - return -1; - } - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_INFO("Could not get shmem lock\n"); - return -1; - } - - for (int i = 0; i < data_len_in_bytes; i++) { - data[i] = shmem_info_p->adsp_changed_index[i]; - } - - release_shmem_lock(__FILE__, __LINE__); - - return 0; -} - -int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, - int data_len_in_bytes) -{ - unsigned int byte_changed, bit_changed; - union param_value_u *param_value = (union param_value_u *) value; - - if (!shmem_info_p) { - return -1; - } - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_INFO("Could not get shmem lock\n"); - return -1; - } - - *param_value = shmem_info_p->params_val[param]; - - /*also clear the index since we are holding the lock*/ - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; - - release_shmem_lock(__FILE__, __LINE__); - - return 0; -} - -int px4muorb_topic_advertised(const char *topic_name) -{ - int rc = 0; - PX4_INFO("TEST px4muorb_topic_advertised of [%s] on remote side...", topic_name); - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - - if (rxHandler != nullptr) { - rc = rxHandler->process_remote_topic(topic_name, 1); - - } else { - rc = -1; - } - - return rc; -} - -int px4muorb_topic_unadvertised(const char *topic_name) -{ - int rc = 0; - PX4_INFO("TEST px4muorb_topic_unadvertised of [%s] on remote side...", topic_name); - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - - if (rxHandler != nullptr) { - rc = rxHandler->process_remote_topic(topic_name, 0); - - } else { - rc = -1; - } - - return rc; -} - -int px4muorb_add_subscriber(const char *name) -{ - int rc = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - channel->AddRemoteSubscriber(name); - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - - if (rxHandler != nullptr) { - rc = rxHandler->process_add_subscription(name, 0); - - if (rc != OK) { - channel->RemoveRemoteSubscriber(name); - } - - } else { - rc = -1; - } - - return rc; -} - -int px4muorb_remove_subscriber(const char *name) -{ - int rc = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - channel->RemoveRemoteSubscriber(name); - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - - if (rxHandler != nullptr) { - rc = rxHandler->process_remove_subscription(name); - - } else { - rc = -1; - } - - return rc; -} - -int px4muorb_send_topic_data(const char *name, const uint8_t *data, - int data_len_in_bytes) -{ - int rc = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - - if (rxHandler != nullptr) { - rc = rxHandler->process_received_message(name, data_len_in_bytes, - (uint8_t *) data); - - } else { - rc = -1; - } - - return rc; -} - -int px4muorb_is_subscriber_present(const char *topic_name, int *status) -{ - int rc = 0; - int32_t local_status = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - rc = channel->is_subscriber_present(topic_name, &local_status); - - if (rc == 0) { - *status = (int) local_status; - } - - return rc; -} - -int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, - uint8_t *data, int data_len_in_bytes, int *bytes_returned) -{ - int rc = 0; - int32_t local_msg_type = 0; - int32_t local_bytes_returned = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); - rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, - data_len_in_bytes, &local_bytes_returned); - *msg_type = (int) local_msg_type; - *bytes_returned = (int) local_bytes_returned; - return rc; -} - -int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, - int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count) -{ - int rc = 0; - int32_t local_bytes_returned = 0; - int32_t local_topic_count = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); - rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, - &local_bytes_returned, &local_topic_count); - *returned_length_in_bytes = (int) local_bytes_returned; - *topic_count = (int) local_topic_count; - return rc; -} - -int px4muorb_unblock_recieve_msg(void) -{ - int rc = 0; - uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); - rc = channel->unblock_get_data_method(); - return rc; -} diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp deleted file mode 100644 index e9f039fdd1..0000000000 --- a/src/modules/muorb/adsp/px4muorb.hpp +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#pragma once - -#include - -extern "C" { - - int px4muorb_orb_initialize() __EXPORT; - - int px4muorb_set_absolute_time_offset(int32_t time_diff_us) __EXPORT; - - int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT; - - int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT; - - int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) __EXPORT; - - int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT; - - int px4muorb_topic_advertised(const char *name) __EXPORT; - - int px4muorb_topic_unadvertised(const char *name) __EXPORT; - - int px4muorb_add_subscriber(const char *name) __EXPORT; - - int px4muorb_remove_subscriber(const char *name) __EXPORT; - - int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; - - int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; - - int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, - int *bytes_returned) __EXPORT; - - int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, - int *length_in_bytes, int *topic_count) __EXPORT; - - int px4muorb_unblock_recieve_msg(void) __EXPORT; - -} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp deleted file mode 100644 index 4e0f07a0b8..0000000000 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ /dev/null @@ -1,715 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include "uORBFastRpcChannel.hpp" -#include -#include -#include -#include - -// static initialization. -uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; - -static unsigned long _dropped_pkts; -static unsigned long _get_min = 0xFFFFFF; -static unsigned long _get_max = 0; -static unsigned long _min_q = 200; -static unsigned long _max_q = 0; -static unsigned long _avg_q = 0; -static unsigned long _count = 0; -static unsigned long _get_bulk_min = 0xFFFFFF; -static unsigned long _get_bulk_max = 0; -static unsigned long _bulk_topic_count_min = 0xFFFFFF; -static unsigned long _bulk_topic_count_max = 0; - -//============================================================================== -//============================================================================== -uORB::FastRpcChannel::FastRpcChannel() - : _RxHandler(0) - , _DataQInIndex(0) - , _DataQOutIndex(0) - , _ControlQInIndex(0) - , _ControlQOutIndex(0) -{ - for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) { - _DataMsgQueue[i]._MaxBufferSize = 0; - _DataMsgQueue[i]._Length = 0; - _DataMsgQueue[i]._Buffer = 0; - } - - _RemoteSubscribers.clear(); -} - -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::topic_advertised(const char *messageName) -{ - return control_msg_queue_add(_CONTROL_MSG_TYPE_ADVERTISE, messageName); -} - -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::topic_unadvertised(const char *messageName) -{ - return control_msg_queue_add(_CONTROL_MSG_TYPE_UNADVERTISE, messageName); -} - -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::control_msg_queue_add(int32_t msgtype, const char *messageName) -{ - int16_t rc = 0; - hrt_abstime t1, t2; - static hrt_abstime check_time = 0; - - t1 = hrt_absolute_time(); - _QueueMutex.lock(); - bool overwriteData = false; - - if (IsControlQFull()) { - // queue is full. Overwrite the oldest data. - _ControlQOutIndex++; - - if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { - _ControlQOutIndex = 0; - } - - overwriteData = true; - _dropped_pkts++; - } - - _ControlMsgQueue[ _ControlQInIndex ]._Type = msgtype; - _ControlMsgQueue[ _ControlQInIndex ]._MsgName = messageName; - - _ControlQInIndex++; - - if (_ControlQInIndex == _MAX_MSG_QUEUE_SIZE) { - _ControlQInIndex = 0; - } - - // the assumption here is that each caller reads only one data from either control or data queue. - if (ControlQSize() == 1 && DataQSize() == 0) { // post it only of the queue moves from empty to available. - _DataAvailableSemaphore.post(); - } - - if ((unsigned long)ControlQSize() < _min_q) { _min_q = (unsigned long)ControlQSize(); } - - if ((unsigned long)ControlQSize() > _max_q) { _max_q = (unsigned long)ControlQSize(); } - - _count++; - _avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(ControlQSize()))) / (double)(_count); - - _QueueMutex.unlock(); - t2 = hrt_absolute_time(); - - if ((unsigned long)(t2 - check_time) > 10000000) { - //PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1)); - //PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count); - _min_q = _MAX_MSG_QUEUE_SIZE * 2; - _max_q = 0; - _avg_q = 0; - _count = 0; - check_time = t2; - } - - return rc; -} - -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) -{ - int16_t rc = 0; - _Subscribers.push_back(messageName); - PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName); - return rc; -} - -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName) -{ - int16_t rc = 0; - _Subscribers.remove(messageName); - - return rc; -} - -int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status) -{ - int16_t rc = 0; - - if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) { - *status = 1; - //PX4_DEBUG("******* Found subscriber for message[%s]....", messageName); - - } else { - *status = 0; - //PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size()); - int i = 0; - - for (std::list::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) { - if (*it == messageName) { - PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i); - } - - ++i; - } - } - - return rc; -} - -int16_t uORB::FastRpcChannel::unblock_get_data_method() -{ - PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()"); - _DataAvailableSemaphore.post(); - return 0; -} -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) -{ - _RxHandler = handler; - return 0; -} - - -//============================================================================== -//============================================================================== -int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) -{ - int16_t rc = 0; - hrt_abstime t1, t2; - static hrt_abstime check_time = 0; - - if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { - //there is no-remote subscriber. So do not queue the message. - return rc; - } - - t1 = hrt_absolute_time(); - _QueueMutex.lock(); - bool overwriteData = false; - - if (IsDataQFull()) { - // queue is full. Overwrite the oldest data. - //PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]", - // _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE); - _DataQOutIndex++; - - if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { - _DataQOutIndex = 0; - } - - overwriteData = true; - _dropped_pkts++; - } - - // now check to see if the data queue's buffer size if large enough to memcpy the data. - // if not, delete the old buffer and re-create a new buffer of larger size. - check_and_expand_data_buffer(_DataQInIndex, length); - - // now memcpy the data to the buffer. - memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length); - _DataMsgQueue[ _DataQInIndex ]._Length = length; - _DataMsgQueue[ _DataQInIndex ]._MsgName = messageName; - - _DataQInIndex++; - - if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) { - _DataQInIndex = 0; - } - - // the assumption here is that each caller reads only one data from either control or data queue. - //if (!overwriteData) { - if (DataQSize() == 1 && ControlQSize() == 0) { // post it only of the queue moves from empty to available. - _DataAvailableSemaphore.post(); - } - - if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); } - - if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); } - - _count++; - _avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(DataQSize()))) / (double)(_count); - - _QueueMutex.unlock(); - t2 = hrt_absolute_time(); - - if ((unsigned long)(t2 - check_time) > 10000000) { - //PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1)); - //PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count); - _min_q = _MAX_MSG_QUEUE_SIZE * 2; - _max_q = 0; - _avg_q = 0; - _count = 0; - check_time = t2; - } - - return rc; -} - -//============================================================================== -//============================================================================== -void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length) -{ - if (_DataMsgQueue[ index ]._MaxBufferSize < length) { - // create a new buffer of size length and delete old buffer. - if (_DataMsgQueue[ index ]._Buffer != 0) { - delete _DataMsgQueue[ index ]._Buffer; - } - - _DataMsgQueue[ index ]._Buffer = new uint8_t[ length ]; - - if (_DataMsgQueue[ index ]._Buffer == 0) { - PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length); - _DataMsgQueue[ index ]._MaxBufferSize = 0; - return; - } - - _DataMsgQueue[ index ]._MaxBufferSize = length; - } -} - -int32_t uORB::FastRpcChannel::DataQSize() -{ - int32_t rc; - rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE; - rc %= _MAX_MSG_QUEUE_SIZE; - return rc; -} - -int32_t uORB::FastRpcChannel::ControlQSize() -{ - int32_t rc; - rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE; - rc %= _MAX_MSG_QUEUE_SIZE; - return rc; -} - -bool uORB::FastRpcChannel::IsControlQFull() -{ - return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); -} - -bool uORB::FastRpcChannel::IsControlQEmpty() -{ - return (ControlQSize() == 0); -} - -bool uORB::FastRpcChannel::IsDataQFull() -{ - return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); -} - -bool uORB::FastRpcChannel::IsDataQEmpty() -{ - return (DataQSize() == 0); -} - -int16_t uORB::FastRpcChannel::get_data -( - int32_t *msg_type, - char *topic_name, - int32_t topic_name_len, - uint8_t *data, - int32_t data_len_in_bytes, - int32_t *bytes_returned -) -{ - int16_t rc = 0; - PX4_DEBUG("Get data should not be called..."); - return -1; - // wait for data availability - static hrt_abstime check_time = 0; - hrt_abstime t1 = hrt_absolute_time(); - _DataAvailableSemaphore.wait(); - // hrt_abstime t2 = hrt_absolute_time(); - _QueueMutex.lock(); - - if (DataQSize() != 0 || ControlQSize() != 0) { - if (ControlQSize() > 0) { - // read the first element of the Control Queue. - *msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type; - - if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) { - memcpy - ( - topic_name, - _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(), - _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() - ); - - topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0; - - *bytes_returned = 0; - - _ControlQOutIndex++; - - if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { - _ControlQOutIndex = 0; - } - - } else { - PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]", - topic_name_len, - _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() - ); - rc = -1; - } - - } else { - // read the first element of the Control Queue. - *msg_type = _DATA_MSG_TYPE; - - if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) || - (_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) { - memcpy - ( - topic_name, - _DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(), - _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() - ); - - topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0; - - *bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length; - memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length); - - _DataQOutIndex++; - - if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { - _DataQOutIndex = 0; - } - - } else { - PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ", - topic_name_len, - _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() - ); - PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ", - data_len_in_bytes, - _DataMsgQueue[ _DataQOutIndex ]._Length - ); - - rc = -1; - } - } - - } else { - PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues"); - rc = -1; - } - - _QueueMutex.unlock(); - hrt_abstime t3 = hrt_absolute_time(); - - if ((unsigned long)(t3 - t1) > _get_max) { _get_max = (unsigned long)(t3 - t1); } - - if ((unsigned long)(t3 - t1) < _get_min) { _get_min = (unsigned long)(t3 - t1); } - - if ((unsigned long)(t3 - check_time) > 1000000) { - if (rc != 0) { - topic_name[0] = '\0'; - } - - /* - PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2, - (unsigned long)t3); - PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); - PX4_DEBUG("ADSP RPC Stats: _get_min: %lu _get_max: %lu _dropped_pkts: %lu", _get_min, _get_max, _dropped_pkts); - */ - check_time = t3; - } - - return rc; -} - - -int16_t uORB::FastRpcChannel::get_bulk_data -( - uint8_t *buffer, - int32_t max_buffer_in_bytes, - int32_t *returned_bytes, - int32_t *topic_count -) -{ - int16_t rc = 0; - // wait for data availability - static hrt_abstime check_time = 0; - hrt_abstime t1 = hrt_absolute_time(); - _DataAvailableSemaphore.wait(); - //hrt_abstime t2 = hrt_absolute_time(); - - _QueueMutex.lock(); - - int32_t bytes_copied = 0; - int32_t copy_result = 0; - *returned_bytes = 0; - *topic_count = 0; - int32_t topic_count_to_return = 0; - - if (DataQSize() != 0 || ControlQSize() != 0) { - if (DataQSize() != 0) { - //PX4_DEBUG( "get_bulk_data: QSize: %d", DataQSize() ); - topic_count_to_return = DataQSize(); - - while (DataQSize() != 0) { - // this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait. - //_DataAvailableSemaphore.wait(); - if (get_msg_size_at(true, _DataQOutIndex) < (max_buffer_in_bytes - bytes_copied)) { - // there is enough space in the buffer, copy the data. - //PX4_DEBUG( "Coping Data to buffer..." ); - copy_result = copy_msg_to_buffer(true, _DataQOutIndex, buffer, bytes_copied, max_buffer_in_bytes); - - if (copy_result == -1) { - if (bytes_copied == 0) { - rc = -1; - } - - break; - - } else { - //PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\ - // buffer[bytes_copied], \ - // buffer[bytes_copied+1], \ - // buffer[bytes_copied+2], \ - // buffer[bytes_copied+3] ); - bytes_copied += copy_result; - (*topic_count)++; - *returned_bytes = bytes_copied; - _DataQOutIndex++; - - if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { - _DataQOutIndex = 0; - } - } - - } else { - if (bytes_copied == 0) { - rc = -1; - PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned"); - - } else { - PX4_DEBUG("Exiting out of the while loop..."); - } - - break; - } - } - } - - if (ControlQSize() != 0) { - //PX4_DEBUG( "get_bulk_data: QSize: %d", ControlQSize() ); - topic_count_to_return += ControlQSize(); - - while (ControlQSize() != 0) { - // this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait. - //_DataAvailableSemaphore.wait(); - if (get_msg_size_at(false, _ControlQOutIndex) < (max_buffer_in_bytes - bytes_copied)) { - // there is enough space in the buffer, copy the data. - //PX4_DEBUG( "Coping Control msg to buffer..." ); - copy_result = copy_msg_to_buffer(false, _ControlQOutIndex, buffer, bytes_copied, max_buffer_in_bytes); - - if (copy_result == -1) { - if (bytes_copied == 0) { - rc = -1; - } - - break; - - } else { - //PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\ - // buffer[bytes_copied], \ - // buffer[bytes_copied+1], \ - // buffer[bytes_copied+2], \ - // buffer[bytes_copied+3] ); - bytes_copied += copy_result; - (*topic_count)++; - *returned_bytes = bytes_copied; - _ControlQOutIndex++; - - if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { - _ControlQOutIndex = 0; - } - } - - } else { - if (bytes_copied == 0) { - rc = -1; - PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned"); - - } else { - PX4_DEBUG("Exiting out of the while loop..."); - } - - break; - } - } - } - - } else { - PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues"); - rc = -1; - } - - if (topic_count_to_return != *topic_count) { - PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count); - } - - _QueueMutex.unlock(); - hrt_abstime t3 = hrt_absolute_time(); - - if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); } - - if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); } - - if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); } - - if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); } - - if ((unsigned long)(t3 - check_time) > 10000000) { - //PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); - //PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); - //PX4_DEBUG("ADSP RPC Stats: _get_bulk_min: %lu _get_bulk_max: %lu _dropped_pkts: %lu", _get_bulk_min, _get_bulk_max, - // _dropped_pkts); - //PX4_DEBUG(" .... topic_count_min: %lu topic_count_max: %lu", _bulk_topic_count_min, _bulk_topic_count_max); - _get_bulk_max = 0; - _get_bulk_min = 0xFFFFFF; - _bulk_topic_count_min = 0xFFFFFF; - _bulk_topic_count_max = 0; - check_time = t3; - } - - //PX4_DEBUG( "Returning topics: %d bytes_returned: %d", *topic_count, *returned_bytes ); - return rc; -} - -int32_t uORB::FastRpcChannel::get_msg_size_at(bool isData, int32_t index) -{ - // the assumption here is that this is called within the context of semaphore, - // hence lock/unlock is not needed. - int32_t rc = 0; - - if (isData) { - rc += _DataMsgQueue[ index ]._Length; - rc += _DataMsgQueue[ index ]._MsgName.size() + 1; - - } else { - rc += _ControlMsgQueue[ index ]._MsgName.size() + 1; - } - - rc += _PACKET_HEADER_SIZE; - return rc; -} - -int32_t uORB::FastRpcChannel::copy_msg_to_buffer(bool isData, int32_t src_index, uint8_t *dst_buffer, int32_t offset, - int32_t dst_buffer_len) -{ - int32_t rc = -1; - - // before calling this method the following are assumed: - // * sem_lock is acquired for data protection - // * the dst_buffer is validated to - - uint16_t msg_size = (isData ? - (uint16_t)(_DataMsgQueue[ src_index ]._MsgName.size()) : - (uint16_t)(_ControlMsgQueue[ src_index ]._MsgName.size())); - - // compute the different offsets to pack the packets. - int32_t field_header_offset = offset; - int32_t field_topic_name_offset = field_header_offset + sizeof(struct BulkTransferHeader); - int32_t field_data_offset = field_topic_name_offset + msg_size + 1; - - int16_t msg_type = isData ? _DATA_MSG_TYPE : _ControlMsgQueue[ src_index ]._Type; - - struct BulkTransferHeader header = { (uint16_t)msg_type, (uint16_t)(msg_size + 1), - (uint16_t)(isData ? (_DataMsgQueue[ src_index ]._Length) : 0) - }; - - - //PX4_DEBUG( "Offsets: header[%d] name[%d] data[%d]", - // field_header_offset, - // field_topic_name_offset, - // field_data_offset ); - - if (isData && (field_data_offset + _DataMsgQueue[ src_index ]._Length) < dst_buffer_len) { - memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header)); - // pack the data here. - memmove - ( - &(dst_buffer[field_topic_name_offset]), - _DataMsgQueue[ src_index ]._MsgName.c_str(), - _DataMsgQueue[ src_index ]._MsgName.size() - ); - - if (_DataMsgQueue[ src_index ]._MsgName.size() == 0) { - PX4_WARN("########## Error MsgName cannot be zero: "); - } - - dst_buffer[ field_topic_name_offset + _DataMsgQueue[ src_index ]._MsgName.size()] = '\0'; - memmove(&(dst_buffer[field_data_offset]), _DataMsgQueue[ src_index ]._Buffer, _DataMsgQueue[ src_index ]._Length); - rc = field_data_offset + _DataMsgQueue[ src_index ]._Length - offset; - - } else if (field_data_offset < dst_buffer_len) { //This is a control message - memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header)); - // pack the data here. - memmove - ( - &(dst_buffer[field_topic_name_offset]), - _ControlMsgQueue[ src_index ]._MsgName.c_str(), - _ControlMsgQueue[ src_index ]._MsgName.size() - ); - - if (_ControlMsgQueue[ src_index ]._MsgName.size() == 0) { - PX4_WARN("########## Error MsgName cannot be zero: "); - } - - dst_buffer[ field_topic_name_offset + _ControlMsgQueue[ src_index ]._MsgName.size()] = '\0'; - rc = field_data_offset - offset; - - } else { - PX4_WARN("Error coping the Msg to dst buffer, insuffienct space. "); - - if (isData) { - PX4_WARN("Data... offset[%ld] len[%ld] data_msg_len[%ld]", - offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length); - - } else { - PX4_WARN("ControlMsg... offset[%ld] len[%ld]", - offset, dst_buffer_len, (field_data_offset - offset)); - } - } - - return rc; -} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp deleted file mode 100644 index c83b631266..0000000000 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp +++ /dev/null @@ -1,301 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#ifndef _uORBFastRpcChannel_hpp_ -#define _uORBFastRpcChannel_hpp_ - -#include -#include -#include -#include "uORB/uORBCommunicator.hpp" -#include -#include -#include - -namespace uORB -{ -class FastRpcChannel; -} - -class uORB::FastRpcChannel : public uORBCommunicator::IChannel -{ -public: - /** - * static method to get the IChannel Implementor. - */ - static uORB::FastRpcChannel *GetInstance() - { - return &(_Instance); - } - - /** - * @brief Interface to notify the remote entity of a topic being advertised. - * - * @param messageName - * This represents the uORB message name(aka topic); This message name should be - * globally unique. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t topic_advertised(const char *messageName); - - /** - * @brief Interface to notify the remote entity of a topic being unadvertised - * and is no longer publishing messages. - * - * @param messageName - * This represents the uORB message name(aka topic); This message name should be - * globally unique. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t topic_unadvertised(const char *messageName); - - /** - * @brief Interface to notify the remote entity of interest of a - * subscription for a message. - * - * @param messageName - * This represents the uORB message name; This message name should be - * globally unique. - * @param msgRate - * The max rate at which the subscriber can accept the messages. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); - - - /** - * @brief Interface to notify the remote entity of removal of a subscription - * - * @param messageName - * This represents the uORB message name; This message name should be - * globally unique. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not necessarily mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t remove_subscription(const char *messageName); - - /** - * Register Message Handler. This is internal for the IChannel implementer* - */ - virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); - - - //========================================================================= - // INTERFACES FOR Data messages - //========================================================================= - - /** - * @brief Sends the data message over the communication link. - * @param messageName - * This represents the uORB message name; This message name should be - * globally unique. - * @param length - * The length of the data buffer to be sent. - * @param data - * The actual data to be sent. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); - - //Function to return the data to krait. - int16_t get_data - ( - int32_t *msg_type, - char *topic_name, - int32_t topic_name_len, - uint8_t *data, - int32_t data_len_in_bytes, - int32_t *bytes_returned - ); - - int16_t get_bulk_data(uint8_t *buffer, int32_t max_size_in_bytes, int32_t *returned_bytes, int32_t *topic_count); - - // function to check if there are subscribers for a topic on adsp. - int16_t is_subscriber_present(const char *messageName, int32_t *status); - - // function to release the blocking semaphore for get_data method. - int16_t unblock_get_data_method(); - - uORBCommunicator::IChannelRxHandler *GetRxHandler() - { - return _RxHandler; - } - - void AddRemoteSubscriber(const std::string &messageName) - { - _RemoteSubscribers.insert(messageName); - } - void RemoveRemoteSubscriber(const std::string &messageName) - { - _RemoteSubscribers.erase(messageName); - } - -private: // data members - static uORB::FastRpcChannel _Instance; - uORBCommunicator::IChannelRxHandler *_RxHandler; - - /// data structure to store the messages to be retrived by Krait. - static const int32_t _MAX_MSG_QUEUE_SIZE = 100; - static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; - static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; - static const int32_t _DATA_MSG_TYPE = 3; - static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4; - static const int32_t _CONTROL_MSG_TYPE_UNADVERTISE = 5; - - static const int32_t _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES = 2; - static const int32_t _PACKET_FIELD_DATA_LEN_IN_BYTES = 2; - static const int32_t _PACKET_HEADER_SIZE = 1 + //first byte is the MSG Type - _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES + _PACKET_FIELD_DATA_LEN_IN_BYTES; - - struct FastRpcDataMsg { - int32_t _MaxBufferSize; - int32_t _Length; - uint8_t *_Buffer; - std::string _MsgName; - }; - - struct FastRpcControlMsg { - int32_t _Type; - std::string _MsgName; - }; - - struct BulkTransferHeader { - uint16_t _MsgType; - uint16_t _MsgNameLen; - uint16_t _DataLen; - }; - - - struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; - int32_t _DataQInIndex; - int32_t _DataQOutIndex; - - struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; - int32_t _ControlQInIndex; - int32_t _ControlQOutIndex; - - std::list _Subscribers; - - //utility classes - class Mutex - { - public: - Mutex() - { - sem_init(&_Sem, 0, 1); - } - ~Mutex() - { - sem_destroy(&_Sem); - } - void lock() - { - sem_wait(&_Sem); - } - void unlock() - { - sem_post(&_Sem); - } - private: - sem_t _Sem; - - Mutex(const Mutex &); - - Mutex &operator=(const Mutex &); - }; - - class Semaphore - { - public: - Semaphore() - { - sem_init(&_Sem, 0, 0); - /* _Sem use case is a signal */ - px4_sem_setprotocol(&_Sem, SEM_PRIO_NONE); - } - ~Semaphore() - { - sem_destroy(&_Sem); - } - void post() - { - sem_post(&_Sem); - } - void wait() - { - sem_wait(&_Sem); - } - private: - sem_t _Sem; - Semaphore(const Semaphore &); - Semaphore &operator=(const Semaphore &); - - }; - - Mutex _QueueMutex; - Semaphore _DataAvailableSemaphore; - -private://class members. - /// constructor. - FastRpcChannel(); - - void check_and_expand_data_buffer(int32_t index, int32_t length); - - bool IsControlQFull(); - bool IsControlQEmpty(); - bool IsDataQFull(); - bool IsDataQEmpty(); - int32_t DataQSize(); - int32_t ControlQSize(); - - int32_t get_msg_size_at(bool isData, int32_t index); - int32_t copy_msg_to_buffer(bool isData, int32_t src_index, uint8_t *dst_buffer, int32_t offset, int32_t dst_buffer_len); - int16_t control_msg_queue_add(int32_t msgtype, const char *messageName); - - std::set _RemoteSubscribers; -}; - -#endif /* _uORBFastRpcChannel_hpp_ */ diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt deleted file mode 100644 index aecc79fd24..0000000000 --- a/src/modules/muorb/krait/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. -# -############################################################################ -include(hexagon_sdk) - -px4_add_module( - MODULE modules__muorb__krait - MAIN muorb - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable - INCLUDES - ${HEXAGON_SDK_INCLUDES} - ${PX4_BINARY_DIR}/platforms/posix - SRCS - uORBKraitFastRpcChannel.cpp - px4muorb_KraitRpcWrapper.cpp - muorb_main.cpp - ) diff --git a/src/modules/muorb/krait/muorb_main.cpp b/src/modules/muorb/krait/muorb_main.cpp deleted file mode 100644 index c7dec2a2c3..0000000000 --- a/src/modules/muorb/krait/muorb_main.cpp +++ /dev/null @@ -1,103 +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. - * - ****************************************************************************/ - -#include -#include "modules/uORB/uORBManager.hpp" -#include "uORBKraitFastRpcChannel.hpp" - -extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } - -static void usage() -{ - warnx("Usage: muorb 'start', 'stop', 'status'"); -} - - -int -muorb_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return -EINVAL; - } - - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { - if (uORB::KraitFastRpcChannel::isInstance()) { - PX4_WARN("muorb already running"); - - } else { - // register the fast rpc channel with UORB. - uORB::Manager::get_instance()->set_uorb_communicator(uORB::KraitFastRpcChannel::GetInstance()); - - // start the KaitFastRPC channel thread. - uORB::KraitFastRpcChannel::GetInstance()->Start(); - } - - return OK; - - } - - if (!strcmp(argv[1], "stop")) { - - if (uORB::KraitFastRpcChannel::isInstance()) { - uORB::KraitFastRpcChannel::GetInstance()->Stop(); - - } else { - PX4_WARN("muorb not running"); - } - - return OK; - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) { - if (uORB::KraitFastRpcChannel::isInstance()) { - PX4_WARN("muorb running"); - - } else { - PX4_WARN("muorb not running"); - } - - return OK; - } - - usage(); - return -EINVAL; -} diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp deleted file mode 100644 index a61606376c..0000000000 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ /dev/null @@ -1,362 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2016 Ramakrishna Kintada. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include "px4muorb_KraitRpcWrapper.hpp" -#include -#include -#include -#include -#include -#include - -using namespace px4muorb; - -/* Flags applied to the allocation of the shared memory for RPC */ -#define MUORB_KRAIT_FASTRPC_MEM_FLAGS 0 - -/* The ID of the HEAP to be used when allocating shared memory */ -//TODO This heap id is used for test purposes. We need to find out the correct one. -#define MUORB_KRAIT_FASTRPC_HEAP_ID 22 - -static char *_TopicNameBuffer = 0; -static const int32_t _MAX_TOPIC_NAME_BUFFER = 256; - -static uint8_t *_DataBuffer = 0; -static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048; - -static bool _Initialized = false; - -// These numbers are based off the fact each fastrpc call for 64K packet is 94us. -// hence we are trying to allocation 64K of byte buffers. -static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024; -static const uint32_t _MAX_TOPICS = 64; -static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = - _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; -static uint8_t *_BulkTransferBuffer = 0; - -unsigned char *adsp_changed_index = 0; - -// The DSP timer can be read from this file. -#define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer" - -/** - * Helper function to get timer difference between time on DSP and appsproc side. - * Usually the DSP gets started around 2s before the appsproc (Linux) side and - * therefore the clocks are not in sync. We change the clock on the DSP side but - * for this we need to find the offset first and then tell code on the DSP side. - * - * @param time_diff_us: pointer to time offset to set. - * @return: 0 on success, < 0 on error. - */ -int calc_timer_diff_to_dsp_us(int32_t *time_diff_us); - -int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) -{ -#if defined(__PX4_POSIX_EXCELSIOR) - *time_diff_us = 0; - return 0; -#endif - int fd = open(DSP_TIMER_FILE, O_RDONLY); - - if (fd < 0) { - PX4_ERR("Could not open DSP timer file %s.", DSP_TIMER_FILE); - return -1; - } - - char buffer[21] {}; - int bytes_read = read(fd, buffer, sizeof(buffer)); - - if (bytes_read < 0) { - PX4_ERR("Could not read DSP timer file %s.", DSP_TIMER_FILE); - close(fd); - return -2; - } - - // Do this call right after reading to avoid latency here. - timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu - + (ts.tv_nsec / 1000); - - close(fd); - - uint64_t time_dsp; - int ret = sscanf(buffer, "%llx", &time_dsp); - - if (ret < 0) { - PX4_ERR("Could not parse DSP timer."); - return -3; - } - - // The clock count needs to get converted to us. - // The magic value of 19.2 was provided by Qualcomm. - time_dsp /= 19.2; - - // Before casting to in32_t, check if it fits. - uint64_t abs_diff = - (time_appsproc > time_dsp) ? - (time_appsproc - time_dsp) : (time_dsp - time_appsproc); - - if (abs_diff > INT32_MAX) { - PX4_ERR("Timer difference too big"); - return -4; - } - - *time_diff_us = time_appsproc - time_dsp; - - PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", - time_dsp, time_appsproc); - PX4_DEBUG("found time_diff: %li us, %.6f s", - *time_diff_us, ((double)*time_diff_us) / 1e6); - - return 0; -} - -bool px4muorb::KraitRpcWrapper::Initialize() -{ - bool rc = true; - - PX4_DEBUG("%s Now calling rpcmem_init...", __FUNCTION__); - rpcmem_init(); - - PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__); - - _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, - _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)); - rc = (_BulkTransferBuffer != NULL) ? true : false; - - if (!rc) { - PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", - __FUNCTION__); - return rc; - - } else { - PX4_DEBUG( - "%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", - __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); - } - - _TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, - _MAX_TOPIC_NAME_BUFFER * sizeof(char)); - - rc = (_TopicNameBuffer != NULL) ? true : false; - - if (!rc) { - PX4_ERR("%s rpcmem_alloc failed! for topic_name_buffer", __FUNCTION__); - rpcmem_free(_BulkTransferBuffer); - return rc; - - } else { - PX4_DEBUG("%s rpcmem_alloc passed for topic_name_buffer", __FUNCTION__); - } - - // now allocate the data buffer. - _DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, - _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); - - rc = (_DataBuffer != NULL) ? true : false; - - if (!rc) { - PX4_ERR("%s rpcmem_alloc failed! for DataBuffer", __FUNCTION__); - // free the topic name buffer; - rpcmem_free(_BulkTransferBuffer); - rpcmem_free(_TopicNameBuffer); - _TopicNameBuffer = 0; - return rc; - - } else { - PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__); - } - - adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t)); - - rc = (adsp_changed_index != NULL) ? true : false; - - if (!rc) { - PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__); - - } else { - memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t)); - } - - int32_t time_diff_us; - - if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) { - rc = false; - return rc; - } - - // call muorb initialize routine. - if (px4muorb_orb_initialize() != 0) { - PX4_ERR("%s Error calling the uorb fastrpc initalize method..", - __FUNCTION__); - rc = false; - return rc; - } - - // TODO FIXME: remove this check or make it less verbose later - px4muorb_set_absolute_time_offset(time_diff_us); - - uint64_t time_dsp; - px4muorb_get_absolute_time(&time_dsp); - - uint64_t time_appsproc = hrt_absolute_time(); - - int diff = (time_dsp - time_appsproc); - - PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", - time_dsp, time_appsproc, diff); - - _Initialized = true; - return rc; -} - -bool px4muorb::KraitRpcWrapper::Terminate() -{ - if (_BulkTransferBuffer != NULL) { - rpcmem_free(_BulkTransferBuffer); - _BulkTransferBuffer = 0; - } - - if (_TopicNameBuffer != NULL) { - rpcmem_free(_TopicNameBuffer); - _TopicNameBuffer = 0; - } - - if (_DataBuffer != NULL) { - rpcmem_free(_DataBuffer); - _DataBuffer = 0; - } - - if (adsp_changed_index != NULL) { - rpcmem_free(adsp_changed_index); - adsp_changed_index = 0; - } - - _Initialized = false; - return true; -} - -int32_t px4muorb::KraitRpcWrapper::TopicAdvertised(const char *topic) -{ - return ((_Initialized) ? px4muorb_topic_advertised(topic) : -1); -} - -int32_t px4muorb::KraitRpcWrapper::TopicUnadvertised(const char *topic) -{ - return ((_Initialized) ? px4muorb_topic_unadvertised(topic) : -1); -} - -int32_t px4muorb::KraitRpcWrapper::AddSubscriber(const char *topic) -{ - return ((_Initialized) ? px4muorb_add_subscriber(topic) : -1); -} - -int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic) -{ - return (_Initialized ? px4muorb_remove_subscriber(topic) : -1); -} - -int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, - int32_t *status) -{ - return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1); -} - -int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, - int32_t length_in_bytes, const uint8_t *data) -{ - return (_Initialized ? - px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); -} - -int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, - int32_t *length_in_bytes, uint8_t **data) -{ - int32_t rc = -1; - - if (_Initialized) { - rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, - _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, - length_in_bytes); - - if (rc == 0) { - *topic = _TopicNameBuffer; - *data = _DataBuffer; - - } else { - PX4_ERR("ERROR: Getting data from fastRPC link"); - } - - } else { - PX4_ERR("ERROR: FastRpcWrapper Not Initialized"); - } - - return rc; -} - -int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, - int32_t *length_in_bytes, int32_t *topic_count) -{ - int32_t rc = -1; - - if (_Initialized) { - //rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes ); - rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, - _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); - - if (rc == 0) { - *bulk_data = _BulkTransferBuffer; - - } else { - PX4_ERR("ERROR: Getting Bulk data from fastRPC link"); - } - - } else { - PX4_ERR("ERROR: FastRpcWrapper Not Initialized"); - } - - return rc; -} - -int32_t px4muorb::KraitRpcWrapper::UnblockReceiveData() -{ - return (_Initialized ? px4muorb_unblock_recieve_msg() : -1); -} diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp deleted file mode 100644 index 3fec23ff9c..0000000000 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp +++ /dev/null @@ -1,78 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 Ramakrishna Kintada. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#ifndef _px4muorb_KraitRpcWrapper_hpp_ -#define _px4muorb_KraitRpcWrapper_hpp_ -#include - -namespace px4muorb -{ -class KraitRpcWrapper; -} - -class px4muorb::KraitRpcWrapper -{ -public: - /** - * Constructor - */ - KraitRpcWrapper() = default; - - /** - * destructor - */ - ~KraitRpcWrapper() = default; - - /** - * Initiatizes the rpc channel px4 muorb - */ - bool Initialize(); - - /** - * Terminate to clean up the resources. This should be called at program exit - */ - bool Terminate(); - - /** - * Muorb related functions to pub/sub of orb topic from krait to adsp - */ - int32_t TopicAdvertised(const char *topic); - int32_t TopicUnadvertised(const char *topic); - int32_t AddSubscriber(const char *topic); - int32_t RemoveSubscriber(const char *topic); - int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data); - int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data); - int32_t IsSubscriberPresent(const char *topic, int32_t *status); - int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count); - int32_t UnblockReceiveData(); -}; -#endif // _px4muorb_KraitWrapper_hpp_ diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp deleted file mode 100644 index a811db5340..0000000000 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ /dev/null @@ -1,375 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "uORBKraitFastRpcChannel.hpp" -#include -#include -#include -#include -#include -#include - -#define LOG_TAG "uORBKraitFastRpcChannel.cpp" - -uORB::KraitFastRpcChannel *uORB::KraitFastRpcChannel::_InstancePtr = nullptr; - -static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics); - -// static initialization. -static std::string _log_file_name = "./hex_dump.txt"; - -static unsigned long _snd_msg_min = 0xFFFFFF; -static unsigned long _snd_msg_max = 0; -static double _snd_msg_avg = 0.0; -static unsigned long _snd_msg_count = 0; -static unsigned long _overall_snd_min = 0xFFFFFF; -static unsigned long _overall_snd_max = 0; -static double _overall_snd_avg = 0.0; -static unsigned long _overall_snd_count = 0; -static hrt_abstime _log_check_time = 0; -static hrt_abstime _log_check_interval = 10000000; - - -uORB::KraitFastRpcChannel::KraitFastRpcChannel() : - _RxHandler(nullptr), - _ThreadStarted(false), - _ThreadShouldExit(false) -{ - _KraitWrapper.Initialize(); -} - -int16_t uORB::KraitFastRpcChannel::topic_advertised(const char *messageName) -{ - int16_t rc = 0; - PX4_DEBUG("Before calling TopicAdvertised for [%s]\n", messageName); - rc = _KraitWrapper.TopicAdvertised(messageName); - PX4_DEBUG("Response for TopicAdvertised for [%s], rc[%d]\n", messageName, rc); - return rc; -} - -int16_t uORB::KraitFastRpcChannel::topic_unadvertised(const char *messageName) -{ - int16_t rc = 0; - PX4_DEBUG("Before calling TopicUnadvertised for [%s]\n", messageName); - rc = _KraitWrapper.TopicUnadvertised(messageName); - PX4_DEBUG("Response for TopicUnadvertised for [%s], rc[%d]\n", messageName, rc); - return rc; -} - -int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) -{ - int16_t rc = 0; - //PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName); - rc = _KraitWrapper.AddSubscriber(messageName); - //PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc); - return rc; -} - -int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName) -{ - int16_t rc = 0; - //PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName); - rc = _KraitWrapper.RemoveSubscriber(messageName); - //PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc); - return rc; -} - -int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) -{ - _RxHandler = handler; - return 0; -} - -int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) -{ - int16_t rc = 0; - int32_t status = 0; - hrt_abstime t1, t4; - hrt_abstime t2 = 0; - hrt_abstime t3 = 0; - t1 = hrt_absolute_time(); - - if (_AdspSubscriberCache.find(std::string(messageName)) == _AdspSubscriberCache.end()) { - // check the status from adsp. as it is not cached. - if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { - _AdspSubscriberCache[messageName] = status; - _AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time(); - } - - } else { - if ((hrt_absolute_time() - _AdspSubscriberSampleTimestamp[messageName]) > _SubCacheRefreshRate) { - if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { - _AdspSubscriberCache[messageName] = status; - _AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time(); - } - } - } - - if (_AdspSubscriberCache[messageName] > 0) {// there are remote subscribers - t2 = hrt_absolute_time(); - rc = _KraitWrapper.SendData(messageName, length, data); - t3 = hrt_absolute_time(); - _snd_msg_count++; - //PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() ); - - } else { - //PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() ); - } - - t4 = hrt_absolute_time(); - _overall_snd_count++; - - if ((t4 - t1) < _overall_snd_min) { _overall_snd_min = (t4 - t1); } - - if ((t4 - t1) > _overall_snd_max) { _overall_snd_max = (t4 - t1); } - - if (_AdspSubscriberCache[messageName] > 0) { - if ((t3 - t2) < _snd_msg_min) { _snd_msg_min = (t3 - t2); } - - if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); } - - _snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) + - (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); - } - - _overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) + - (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); - - if ((t4 - _log_check_time) > _log_check_interval) { - /* - PX4_DEBUG("SndMsgStats: overall_min: %lu overall_max: %lu snd_msg_min: %lu snd_msg_max: %lu", - _overall_snd_min, _overall_snd_max, - _snd_msg_min, _snd_msg_max); - PX4_DEBUG(".... overall_avg: %f (%lu) snd_msg_avg: %f (%lu)", - _overall_snd_avg, _overall_snd_count, _snd_msg_avg, _snd_msg_count); - */ - _log_check_time = t4; - _overall_snd_min = _snd_msg_min = 0xFFFFFFF; - _overall_snd_max = _snd_msg_max = 0; - _overall_snd_count = _snd_msg_count = 0; - _overall_snd_avg = _snd_msg_avg = 0.0; - } - - //PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc ); - return rc; -} - -void uORB::KraitFastRpcChannel::Start() -{ - _ThreadStarted = true; - _ThreadShouldExit = false; - pthread_attr_t recv_thread_attr; - pthread_attr_init(&recv_thread_attr); - - struct sched_param param; - (void)pthread_attr_getschedparam(&recv_thread_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 80; - (void)pthread_attr_setschedparam(&recv_thread_attr, ¶m); - - pthread_attr_setstacksize(&recv_thread_attr, 4096); - - if (pthread_create(&_RecvThread, &recv_thread_attr, thread_start, (void *)this) != 0) { - PX4_ERR("Error creating the receive thread for muorb"); - - } else { - pthread_setname_np(_RecvThread, "muorb_krait_receiver"); - } - - pthread_attr_destroy(&recv_thread_attr); -} - -void uORB::KraitFastRpcChannel::Stop() -{ - _ThreadShouldExit = true; - _KraitWrapper.UnblockReceiveData(); - //PX4_DEBUG("After calling UnblockReceiveData()...\n"); - pthread_join(_RecvThread, NULL); - //PX4_DEBUG("*** After calling pthread_join...\n"); - _ThreadStarted = false; -} - -void *uORB::KraitFastRpcChannel::thread_start(void *handler) -{ - if (handler != nullptr) { - ((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread(); - } - - return 0; -} - -void uORB::KraitFastRpcChannel::fastrpc_recv_thread() -{ - // sit in while loop. - int32_t rc = 0; - int32_t data_length = 0; - uint8_t *data = nullptr; - unsigned long rpc_min, rpc_max; - unsigned long orb_min, orb_max; - double rpc_avg, orb_avg; - unsigned long count = 0; - rpc_max = orb_max = 0; - rpc_min = orb_min = 0xFFFFFFFF; - rpc_avg = orb_avg = 0.0; - - int32_t num_topics = 0; - - hrt_abstime check_time = 0; - - while (!_ThreadShouldExit) { - hrt_abstime t1, t2, t3; - t1 = hrt_absolute_time(); - rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics); - - t2 = hrt_absolute_time(); - - if (rc == 0) { - //PX4_DEBUG( "Num of topics Received: %d", num_topics ); - int32_t bytes_processed = 0; - - for (int i = 0; i < num_topics; ++i) { - uint8_t *new_pkt = &(data[bytes_processed]); - struct BulkTransferHeader *header = (struct BulkTransferHeader *)new_pkt; - char *messageName = (char *)(new_pkt + sizeof(struct BulkTransferHeader)); - uint16_t check_msg_len = strlen(messageName); - - if (header->_MsgNameLen != (check_msg_len + 1)) { - PX4_ERR("Error: Packing error. Sent Msg Len. of[%d] but strlen returned:[%d]", header->_MsgNameLen, check_msg_len); - PX4_ERR("Error: NumTopics: %d processing topic: %d msgLen[%d] dataLen[%d] data_len[%d] bytes processed: %d", - num_topics, i, header->_MsgNameLen, header->_DataLen, data_length, bytes_processed); - DumpData(data, data_length, num_topics); - break; - } - - uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1); - - if (_RxHandler != nullptr) { - if (header->_MsgType == _DATA_MSG_TYPE) { - //PX4_DEBUG( "Received topic data for: [%s] len[%d]\n", messageName, data_length ); - _RxHandler->process_received_message(messageName, - header->_DataLen, topic_data); - - } else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) { - PX4_DEBUG("Received topic advertise message for: [%s] len[%d]\n", messageName, data_length); - _RxHandler->process_remote_topic(messageName, true); - - } else if (header->_MsgType == _CONTROL_MSG_TYPE_UNADVERTISE) { - PX4_DEBUG("Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length); - _RxHandler->process_remote_topic(messageName, false); - } - } - - bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader); - } - - } else { - PX4_DEBUG("Error: Getting data over fastRPC channel\n"); - break; - } - - t3 = hrt_absolute_time(); - count++; - - if ((unsigned long)(t2 - t1) < rpc_min) { - rpc_min = (unsigned long)(t2 - t1); - } - - if ((unsigned long)(t2 - t1) > rpc_max) { - rpc_max = (unsigned long)(t2 - t1); - } - - if ((unsigned long)(t3 - t2) < orb_min) { - orb_min = (unsigned long)(t3 - t2); - } - - if ((unsigned long)(t3 - t2) > orb_max) { - orb_max = (unsigned long)(t3 - t2); - } - - rpc_avg = ((double)((rpc_avg * (count - 1)) + (unsigned long)(t2 - t1))) / (double)(count); - orb_avg = ((double)((orb_avg * (count - 1)) + (unsigned long)(t3 - t2))) / (double)(count); - - if ((unsigned long)(t3 - check_time) >= 10000000) { - //PX4_DEBUG("Krait RPC Stats : rpc_min: %lu rpc_max: %lu rpc_avg: %f", rpc_min, rpc_max, rpc_avg); - //PX4_DEBUG("Krait RPC(orb) Stats: orb_min: %lu orb_max: %lu orb_avg: %f", orb_min, orb_max, orb_avg); - check_time = t3; - rpc_max = orb_max = 0; - rpc_min = orb_min = 0xFFFFFF; - orb_avg = 0; - rpc_avg = 0; - count = 0; - } - - //PX4_DEBUG("MsgName: %30s, t1: %lu, t2: %lu, t3: %lu, dt1: %lu, dt2: %lu",name, (unsigned long) t1, (unsigned long) t2, (unsigned long) t3, - // (unsigned long) (t2-t1), (unsigned long) (t3-t2)); - } - - PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n"); -} - -void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics) -{ - FILE *fp = fopen(_log_file_name.c_str(), "a+"); - - if (fp == nullptr) { - PX4_ERR("Error unable to open log file[%s]", _log_file_name.c_str()); - return; - } - - fprintf(fp, "===== Data Len[%d] num_topics[%d] ======\n", length, num_topics); - - for (int i = 0; i < length; i += 16) { - int remaining_chars = length - i; - remaining_chars = (remaining_chars >= 16) ? 16 : remaining_chars; - - fprintf(fp, "%p - ", &(buffer[i])); - - for (int j = 0; j < remaining_chars; j++) { - fprintf(fp, " %02X", buffer[i + j]); - - if (j == 7) { - fprintf(fp, " -"); - } - } - - fprintf(fp, " "); - - for (int j = 0; j < remaining_chars; j++) { - fprintf(fp, "%c", (char)buffer[i + j ]); - } - - fprintf(fp, "\n"); - } - - fclose(fp); -} - diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp deleted file mode 100644 index 25fad8e0ed..0000000000 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef _uORBKraitFastRpcChannel_hpp_ -#define _uORBKraitFastRpcChannel_hpp_ - -#include -#include -#include -#include "uORB/uORBCommunicator.hpp" -#include "px4muorb_KraitRpcWrapper.hpp" -#include -#include "drivers/drv_hrt.h" - -namespace uORB -{ -class KraitFastRpcChannel; -} - -class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel -{ -public: - /** - * static method to get the IChannel Implementor. - */ - static uORB::KraitFastRpcChannel *GetInstance() - { - if (_InstancePtr == nullptr) { - _InstancePtr = new uORB::KraitFastRpcChannel(); - } - - return _InstancePtr; - } - - /** - * Static method to check if there is an instance. - */ - static bool isInstance() - { - return (_InstancePtr != nullptr); - } - - /** - * @brief Interface to notify the remote entity of a topic being advertised. - * - * @param messageName - * This represents the uORB message name(aka topic); This message name should be - * globally unique. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t topic_advertised(const char *messageName); - - /** - * @brief Interface to notify the remote entity of a topic being unadvertised - * and is no longer publishing messages. - * - * @param messageName - * This represents the uORB message name(aka topic); This message name should be - * globally unique. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t topic_unadvertised(const char *messageName); - - /** - * @brief Interface to notify the remote entity of interest of a - * subscription for a message. - * - * @param messageName - * This represents the uORB message name; This message name should be - * globally unique. - * @param msgRate - * The max rate at which the subscriber can accept the messages. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); - - - /** - * @brief Interface to notify the remote entity of removal of a subscription - * - * @param messageName - * This represents the uORB message name; This message name should be - * globally unique. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not necessarily mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t remove_subscription(const char *messageName); - - /** - * Register Message Handler. This is internal for the IChannel implementer* - */ - virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); - - - //========================================================================= - // INTERFACES FOR Data messages - //========================================================================= - - /** - * @brief Sends the data message over the communication link. - * @param messageName - * This represents the uORB message name; This message name should be - * globally unique. - * @param length - * The length of the data buffer to be sent. - * @param data - * The actual data to be sent. - * @return - * 0 = success; This means the messages is successfully sent to the receiver - * Note: This does not mean that the receiver as received it. - * otherwise = failure. - */ - virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); - - - void Start(); - void Stop(); - -private: // data members - static uORB::KraitFastRpcChannel *_InstancePtr; - uORBCommunicator::IChannelRxHandler *_RxHandler; - pthread_t _RecvThread; - bool _ThreadStarted; - bool _ThreadShouldExit; - - static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; - static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; - static const int32_t _DATA_MSG_TYPE = 3; - static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4; - static const int32_t _CONTROL_MSG_TYPE_UNADVERTISE = 5; - - struct BulkTransferHeader { - uint16_t _MsgType; - uint16_t _MsgNameLen; - uint16_t _DataLen; - }; - - px4muorb::KraitRpcWrapper _KraitWrapper; - - std::map _AdspSubscriberCache; - std::map _AdspSubscriberSampleTimestamp; - //hrt_abstime _SubCacheSampleTimestamp; - static const hrt_abstime _SubCacheRefreshRate = 1000000; // 1 second; - -private://class members. - /// constructor. - KraitFastRpcChannel(); - - static void *thread_start(void *handler); - - void fastrpc_recv_thread(); - -}; - -#endif /* _uORBKraitFastRpcChannel_hpp_ */ diff --git a/src/modules/muorb/test/CMakeLists.txt b/src/modules/muorb/test/CMakeLists.txt deleted file mode 100644 index 341f2d99ed..0000000000 --- a/src/modules/muorb/test/CMakeLists.txt +++ /dev/null @@ -1,41 +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__muorb__test__muorb_test - MAIN muorb_test - SRCS - muorb_test_main.cpp - muorb_test_start_posix.cpp - muorb_test_example.cpp - DEPENDS - ) diff --git a/src/modules/muorb/test/muorb_test_example.cpp b/src/modules/muorb/test/muorb_test_example.cpp deleted file mode 100644 index 8cb9804c78..0000000000 --- a/src/modules/muorb/test/muorb_test_example.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hello_example.cpp - * Example for Linux - * - * @author Mark Charlebois - */ - -#include "muorb_test_example.h" -#include -#include -#include -#include -#include -#include - -px4::AppState MuorbTestExample::appState; - -int MuorbTestExample::main() -{ - int rc; - appState.setRunning(true); - rc = PingPongTest(); - appState.setRunning(false); - return rc; -} - -int MuorbTestExample::DefaultTest() -{ - int i = 0; - - uORB::Subscription sub_vc{ORB_ID(vehicle_command)}; - uORB::Publication 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 (!pub_id.publish(m_esc_status)) { - PX4_ERR("[%d]Error publishing the esc status message for iter", i); - break; - } - - if (sub_vc.updated()) { - PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); - - if (!sub_vc.copy(&m_vc)) { - PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); - break; - } - - 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; - } - - ++i; - } - - return 0; -} - -int MuorbTestExample::PingPongTest() -{ - int i = 0; - uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; - uORB::Subscription sub_esc_status{ORB_ID(esc_status)}; - - while (!appState.exitRequested()) { - - PX4_INFO("[%d] Doing work...", i); - - if (sub_esc_status.updated()) { - PX4_INFO("[%d]ESC status is updated... reading new value", i); - - if (!sub_esc_status.copy(&m_esc_status)) { - PX4_ERR("[%d]Error calling orb copy for esc status... ", i); - break; - } - - if (!vcmd_pub.publish(m_vc)) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - break; - } - - } else { - PX4_INFO("[%d] esc status topic is not updated", i); - } - - // sleep for 1 sec. - usleep(1000000); - - ++i; - } - - return 0; -} diff --git a/src/modules/muorb/test/muorb_test_example.h b/src/modules/muorb/test/muorb_test_example.h deleted file mode 100644 index 1925ba190f..0000000000 --- a/src/modules/muorb/test/muorb_test_example.h +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hello_example.h - * Example app for Linux - * - * @author Mark Charlebois - */ -#pragma once - -#include -#include "uORB/topics/esc_status.h" -#include "uORB/topics/vehicle_command.h" - -class MuorbTestExample -{ -public: - MuorbTestExample() {} - - ~MuorbTestExample() {} - - int main(); - - static px4::AppState appState; /* track requests to terminate app */ -private: - int DefaultTest(); - int PingPongTest(); - struct esc_status_s m_esc_status; - struct vehicle_command_s m_vc = {}; - -}; diff --git a/src/modules/muorb/test/muorb_test_main.cpp b/src/modules/muorb/test/muorb_test_main.cpp deleted file mode 100644 index 551b18ea64..0000000000 --- a/src/modules/muorb/test/muorb_test_main.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hello_main.cpp - * Example for Linux - * - * @author Mark Charlebois - */ -#include -#include -#include -#include "muorb_test_example.h" -#include -#include "uORB/uORBManager.hpp" -#include "../../../modules/muorb/krait/uORBKraitFastRpcChannel.hpp" - -int PX4_MAIN(int argc, char **argv) -{ - px4::init(argc, argv, "muorb_test"); - - PX4_DEBUG("muorb_test"); - - MuorbTestExample hello; - hello.main(); - - PX4_DEBUG("goodbye"); - return 0; -} diff --git a/src/modules/muorb/test/muorb_test_start_posix.cpp b/src/modules/muorb/test/muorb_test_start_posix.cpp deleted file mode 100644 index 3ca3b2c0ee..0000000000 --- a/src/modules/muorb/test/muorb_test_start_posix.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hello_start_linux.cpp - * - * @author Thomas Gubler - * @author Mark Charlebois - */ -#include "muorb_test_example.h" -#include -#include -#include -#include -#include -#include - -static int daemon_task; /* Handle of deamon task / thread */ - -//using namespace px4; - -extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); - -static void usage() -{ - PX4_DEBUG("usage: muorb_test {start|stop|status}"); -} -int muorb_test_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (MuorbTestExample::appState.isRunning()) { - PX4_DEBUG("already running"); - /* this is not an error */ - return 0; - } - - daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char *const *)argv); - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - MuorbTestExample::appState.requestExit(); - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (MuorbTestExample::appState.isRunning()) { - PX4_DEBUG("is running"); - - } else { - PX4_DEBUG("not started"); - } - - return 0; - } - - usage(); - return 1; -} diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index 127ad6154b..d19927f47f 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -47,12 +47,6 @@ endif() configure_file(simulator_config.h.in simulator_config.h @ONLY) include_directories(${CMAKE_CURRENT_BINARY_DIR}) -set(SIMULATOR_SRCS simulator.cpp) -if (NOT ${PX4_PLATFORM} STREQUAL "qurt") - list(APPEND SIMULATOR_SRCS - simulator_mavlink.cpp) -endif() - px4_add_module( MODULE modules__simulator MAIN simulator @@ -63,7 +57,8 @@ px4_add_module( INCLUDES ${PX4_SOURCE_DIR}/mavlink/include/mavlink SRCS - ${SIMULATOR_SRCS} + simulator.cpp + simulator_mavlink.cpp DEPENDS git_mavlink_v2 conversion