diff --git a/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing b/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing index fc593aed3b5b..1c2874466225 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing +++ b/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing @@ -32,8 +32,10 @@ param set-default PWM_AUX_FUNC4 101 param set-default PWM_MAIN_REV 2 param set-default PWM_AUX_REV 2 +# Enable airspeed sensor +param set-default SENS_EN_SDP3X 1 + # Airspeed parameters -sdp3x start -X -f 400 param set-default ASPD_DO_CHECKS 15 param set-default FW_AIRSPD_MAX 22.0 param set-default FW_AIRSPD_MIN 14.0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker b/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker index 46a6e03e2ff2..8e6358b641bc 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker +++ b/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker @@ -23,7 +23,6 @@ param set-default PWM_MAIN_FUNC2 202 param set-default PWM_MAIN_FUNC4 101 # Airspeed parameters -ms4525do start -X -f 500 param set-default SENS_EN_MS4525DO 1 param set-default ASPD_DO_CHECKS 15 param set-default FW_AIRSPD_MAX 25.0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini b/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini index ff454fcec4c8..cb6089a160dd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini +++ b/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini @@ -71,8 +71,8 @@ param set-default PWM_AUX_FUNC5 205 param set-default PWM_AUX_FUNC6 105 # Start airspeed sensor driver -ms4525do start -X -f 500 -sdp3x start -X -f 500 +param set-default SENS_EN_MS4525DO 1 +param set-default SENS_EN_SDP3X 1 # Airspeed parameters param set-default ASPD_DO_CHECKS 7 diff --git a/boards/ssrc/saluki-pi b/boards/ssrc/saluki-pi index 62fec3456ce9..34fc7f7421a8 160000 --- a/boards/ssrc/saluki-pi +++ b/boards/ssrc/saluki-pi @@ -1 +1 @@ -Subproject commit 62fec3456ce901267a74baaedb1df106b9884168 +Subproject commit 34fc7f7421a873c93da9b2e30089d7e8b666b80c diff --git a/boards/ssrc/saluki-v2 b/boards/ssrc/saluki-v2 index 6a74e0cc9aae..42cbe653acda 160000 --- a/boards/ssrc/saluki-v2 +++ b/boards/ssrc/saluki-v2 @@ -1 +1 @@ -Subproject commit 6a74e0cc9aae9c4623f0e5743dc5393be807a10e +Subproject commit 42cbe653acda38e66acd6e4ee7c6da7ca38930f4 diff --git a/boards/ssrc/saluki-v3 b/boards/ssrc/saluki-v3 index f6f8a7c41860..b8a368ad78b4 160000 --- a/boards/ssrc/saluki-v3 +++ b/boards/ssrc/saluki-v3 @@ -1 +1 @@ -Subproject commit f6f8a7c418602a0c1798c34bff4b12324526f727 +Subproject commit b8a368ad78b45d01edb6da87cd63cfb1f5e4a9db diff --git a/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp b/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp index b98483d2f141..61b74b810455 100644 --- a/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp +++ b/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp @@ -53,7 +53,7 @@ #include #include -static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x28; /* Register address */ diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index d9c89c5821e6..8c87c2e83c3b 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -51,7 +51,7 @@ #define I2C_ADDRESS_2_SDP3X 0x22 #define I2C_ADDRESS_3_SDP3X 0x23 -static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface #define SDP3X_SCALE_TEMPERATURE 200.0f #define SDP3X_RESET_ADDR 0x00 diff --git a/src/systemcmds/telem_test/CMakeLists.txt b/src/systemcmds/telem_test/CMakeLists.txt new file mode 100644 index 000000000000..b6b3e594592d --- /dev/null +++ b/src/systemcmds/telem_test/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2024 Technology Innoavation Institute. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE systemcmds__telem_test + MAIN telem_test + COMPILE_FLAGS + SRCS + telem_test.cpp + DEPENDS + ) diff --git a/src/systemcmds/telem_test/Kconfig b/src/systemcmds/telem_test/Kconfig new file mode 100644 index 000000000000..36a5c5a07e84 --- /dev/null +++ b/src/systemcmds/telem_test/Kconfig @@ -0,0 +1,12 @@ +menuconfig SYSTEMCMDS_TELEM_TEST + bool "telem_test" + default n + ---help--- + Enable support for telemetry uart test + +menuconfig USER_TELEM_TEST + bool "telemetry uart test running as userspace module" + default y + depends on BOARD_PROTECTED && SYSTEMCMDS_TELEM_TEST + ---help--- + Put telemetry uart test in userspace memory diff --git a/src/systemcmds/telem_test/telem_test.cpp b/src/systemcmds/telem_test/telem_test.cpp new file mode 100644 index 000000000000..32330f5222db --- /dev/null +++ b/src/systemcmds/telem_test/telem_test.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Technology Innovation Institute. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include +#include + +static void usage(const char *reason) +{ + if (reason != nullptr) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION("Telemetry UART test tool"); + + PRINT_MODULE_USAGE_NAME("telem_test", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("flush", "Flush UART"); + PRINT_MODULE_USAGE_ARG("", "Uart device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("loopback", "Loopback test between uarts"); + PRINT_MODULE_USAGE_ARG("", "First UART device", false); + PRINT_MODULE_USAGE_ARG("", "Second UART device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("send", "Send string to UART"); + PRINT_MODULE_USAGE_ARG("", "Uart device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("recv", "Receive string from UART"); + PRINT_MODULE_USAGE_ARG("", "UART device", false); +} + +static int test_recv(int uart_recv, char *recv_buf, size_t buf_len) +{ + if (read(uart_recv, recv_buf, buf_len) < 0) { + printf("ERROR read: %d\n", errno); + return 1; + } + + return 0; +} + +static int test_send(int uart_send, const char *send_buf) +{ + if (write(uart_send, send_buf, strlen(send_buf)) < 0) { + printf("ERROR write: %d\n", errno); + return 1; + } + + px4_usleep(200000); + + return 0; +} + +static int loopback(int uart_send, int uart_recv, const char *send_buf) +{ + char recv_buf[15] = {0}; + + if (test_send(uart_send, send_buf)) { + return 1; + } + + if (test_recv(uart_recv, recv_buf, sizeof(recv_buf) - 1)) { + return 1; + } + + if (strncmp(send_buf, recv_buf, strlen(send_buf)) != 0) { + printf("ERROR: send != recv\n"); + printf(" send '%s'\n", send_buf); + printf(" recv '%s'\n", recv_buf); + return 1; + } + + return 0; +} + +static int test_loopback(int uart1, int uart2) +{ + const char *send_str1 = "send_uart_1"; + const char *send_str2 = "send_uart_2"; + + printf("send: first -> second\n"); + + if (loopback(uart1, uart2, send_str1)) { + return 1; + } + + printf("send: second -> first\n"); + + if (loopback(uart2, uart1, send_str2)) { + return 1; + } + + return 0; +} + +extern "C" __EXPORT int telem_test_main(int argc, char *argv[]) +{ + int res = 1; + + if (argc < 3) { + usage(nullptr); + return 1; + } + + const char *test_type = argv[1]; + const char *uart1_dev = argv[2]; + const char *uart2_dev = NULL; + + if (argc > 3) { + uart2_dev = argv[3]; + } + + printf("test type: %s\n", test_type); + + int uart1 = open(uart1_dev, O_RDWR | O_NONBLOCK | O_NOCTTY); + + if (uart1 < 0) { + printf("ERROR: %s open\n", uart1_dev); + return 1; + } + + if (strcmp(test_type, "loopback") == 0) { + printf("first uart: %s\n", uart1_dev); + + if (!uart2_dev) { + usage("ERROR: second uart not specified"); + close(uart1); + return 1; + } + + int uart2 = open(uart2_dev, O_RDWR | O_NONBLOCK | O_NOCTTY); + + if (uart2 < 0) { + printf("ERROR: %s open\n", uart2_dev); + close(uart1); + return 1; + } + + printf("second uart: %s\n", uart2_dev); + + res = test_loopback(uart1, uart2); + + close(uart2); + + } else if (strcmp(test_type, "flush") == 0) { + printf("flush uart: %s\n", uart1_dev); + + res = tcflush(uart1, TCIOFLUSH); + + if (res < 0) { + printf("ERROR tcflush: %d\n", errno); + } + + } else if (strcmp(test_type, "send") == 0) { + printf("uart: %s\n", uart1_dev); + + const char *send_str = "send_uart"; + res = test_send(uart1, send_str); + + printf("sent: '%s'\n", send_str); + + } else if (strcmp(test_type, "recv") == 0) { + printf("uart: %s\n", uart1_dev); + + char recv_buf[20] = {0}; + res = test_recv(uart1, recv_buf, sizeof(recv_buf) - 1); + + printf("recv: '%s'\n", recv_buf); + + } else { + usage("ERROR: test type not supported"); + } + + close(uart1); + return res; +}