diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 6c7a290494..6210bf588c 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -44,7 +44,7 @@ pipeline { ] def snapdragon_builds = [ - target: ["atlflight_eagle_qurt-default", "atlflight_eagle_default"], + target: ["atlflight_eagle_qurt", "atlflight_eagle_default"], image: docker_images.snapdragon, archive: false ] diff --git a/Makefile b/Makefile index bbfda47d39..b121e5ecef 100644 --- a/Makefile +++ b/Makefile @@ -223,10 +223,10 @@ posix_sitl_default: .PHONY: all posix px4_sitl_default all_config_targets all_default_targets # Multi- config targets. -eagle_default: atlflight_eagle_default atlflight_eagle_qurt-default +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-default +excelsior_default: atlflight_excelsior_default atlflight_excelsior_qurt excelsior_rtps: atlflight_excelsior_rtps atlflight_excelsior_qurt-rtps .PHONY: eagle_default eagle_rtps diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt.cmake similarity index 99% rename from boards/atlflight/eagle/qurt-default.cmake rename to boards/atlflight/eagle/qurt.cmake index 7b4c297f17..3e271a25f6 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt.cmake @@ -42,7 +42,7 @@ px4_add_board( PLATFORM qurt VENDOR atlflight MODEL eagle - LABEL qurt-default + LABEL qurt DRIVERS barometer/bmp280 gps diff --git a/boards/atlflight/eagle/scripts/.gitignore b/boards/atlflight/eagle/scripts/.gitignore new file mode 100644 index 0000000000..65f4d26f13 --- /dev/null +++ b/boards/atlflight/eagle/scripts/.gitignore @@ -0,0 +1,2 @@ +minidm.log +px4.log diff --git a/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh b/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh index 24705af69e..beed101566 100755 --- a/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh +++ b/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh @@ -147,7 +147,7 @@ installpx4() { # Reboot the target before beginning the installation echo -e "Rebooting the target..." adb reboot - adb wait-for-devices + adb wait-for-usb-device # Wait a bit longer after bootup, before copying binaries to the target. sleep 30 adb devices @@ -157,8 +157,8 @@ installpx4() { 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-default/platforms/qurt/libpx4.so /usr/share/data/adsp - adb push $workspace/build/atlflight_eagle_qurt-default/platforms/qurt/libpx4muorb_skel.so /usr/share/data/adsp + adb push $workspace/build/atlflight_eagle_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 diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt.cmake similarity index 99% rename from boards/atlflight/excelsior/qurt-default.cmake rename to boards/atlflight/excelsior/qurt.cmake index 47e5030afc..2039e65a85 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt.cmake @@ -42,7 +42,7 @@ px4_add_board( PLATFORM qurt VENDOR atlflight MODEL excelsior - LABEL qurt-default + LABEL qurt DRIVERS barometer/bmp280 gps diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index d9fa782c98..98c7710deb 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -105,7 +105,12 @@ public: */ inline T fetch_add(T num) { +#ifdef __PX4_QURT + // TODO: fix + return _value++; +#else return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); +#endif } /** diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index 1885d1078f..44dc4b1093 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -46,7 +46,13 @@ else() modules__muorb__adsp ${module_libraries} ${df_driver_libs} + df_driver_framework 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/src/px4/common/px4_init.cpp b/platforms/qurt/src/px4/common/px4_init.cpp index a28e01c014..1921d88fe3 100644 --- a/platforms/qurt/src/px4/common/px4_init.cpp +++ b/platforms/qurt/src/px4/common/px4_init.cpp @@ -38,7 +38,7 @@ #include #include -int px4_platform_init(void) +int px4_platform_init() { hrt_init(); diff --git a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt index 292c8a5aa7..5f56c75d41 100644 --- a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt @@ -42,4 +42,5 @@ px4_add_module( git_driverframework df_driver_framework df_ltc2946 + battery ) diff --git a/src/drivers/qshell/qurt/CMakeLists.txt b/src/drivers/qshell/qurt/CMakeLists.txt index 36c3fb4dcb..1133de1361 100644 --- a/src/drivers/qshell/qurt/CMakeLists.txt +++ b/src/drivers/qshell/qurt/CMakeLists.txt @@ -34,9 +34,8 @@ px4_add_module( MODULE drivers__qshell__qurt MAIN qshell SRCS - qshell_main.cpp + qshell_main.cpp qshell_start_qurt.cpp qshell.cpp DEPENDS ) - diff --git a/src/lib/drivers/device/posix/SPI.cpp b/src/lib/drivers/device/posix/SPI.cpp index 90a50ab012..a49ce15187 100644 --- a/src/lib/drivers/device/posix/SPI.cpp +++ b/src/lib/drivers/device/posix/SPI.cpp @@ -140,10 +140,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (result != (int)len) { PX4_ERR("write failed. Reported %d bytes written (%s)", result, strerror(errno)); - return -1; + return PX4_ERROR; } - return 0; + return PX4_OK; } int @@ -183,10 +183,10 @@ SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) if (result != (int)(len * 2)) { PX4_ERR("write failed. Reported %d bytes written (%s)", result, strerror(errno)); - return -1; + return PX4_ERROR; } - return 0; + return PX4_OK; } } // namespace device diff --git a/src/lib/drivers/device/qurt/SPI.cpp b/src/lib/drivers/device/qurt/SPI.cpp index 28b00931d9..316686dd20 100644 --- a/src/lib/drivers/device/qurt/SPI.cpp +++ b/src/lib/drivers/device/qurt/SPI.cpp @@ -43,6 +43,7 @@ #include #include #include + #include "dev_fs_lib_spi.h" #include @@ -79,7 +80,7 @@ SPI::init() { // Open the actual SPI device char dev_path[16]; - snprintf(dev_path, sizeof(dev_path), "dev/spi-%lu", PX4_SPI_DEV_ID(_device)); + 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); @@ -117,7 +118,29 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) 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; @@ -127,10 +150,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (result < 0) { PX4_ERR("transfer error %d", result); - return result; + return PX4_ERROR; } - return result; + return PX4_OK; } int @@ -140,15 +163,28 @@ SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) return -EINVAL; } - // int bits = 16; - // result = ::ioctl(_fd, SPI_IOC_WR_BITS_PER_WORD, &bits); + // 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 (result == -1) { - // PX4_ERR("can’t set 16 bit spi mode"); + // 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; @@ -158,10 +194,10 @@ SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) if (result < 0) { PX4_ERR("transfer error %d", result); - return result; + return PX4_ERROR; } - return result; + return PX4_OK; } } // namespace device