diff --git a/boards/modalai/voxl2/voxl-px4 b/boards/modalai/voxl2/voxl-px4 index c529456d22..9936f48796 100644 --- a/boards/modalai/voxl2/voxl-px4 +++ b/boards/modalai/voxl2/voxl-px4 @@ -13,24 +13,26 @@ else /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh else /bin/echo "Could not find the DSP signature file generation script" + exit 0 fi - exit 0 fi # Make sure to setup all of the needed px4 aliases. cd /usr/bin /bin/ln -f -s px4 px4-muorb -/bin/ln -f -s px4 px4-uorb cd - DAEMON=" " +S="OFF" -while getopts "d" flag +while getopts "ds" flag do case "${flag}" in # Use -d to put PX4 into daemon mode d) DAEMON="-d";; + # Use -s to run self tests + s) S="ON";; esac done -px4 $DAEMON -s /etc/modalai/voxl-px4.config +TESTMODE=$S px4 $DAEMON -s /etc/modalai/voxl-px4.config diff --git a/boards/modalai/voxl2/voxl-px4.config b/boards/modalai/voxl2/voxl-px4.config index a0ab82f1ed..c3deb46000 100755 --- a/boards/modalai/voxl2/voxl-px4.config +++ b/boards/modalai/voxl2/voxl-px4.config @@ -3,4 +3,17 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh +if [ $TESTMODE = "ON" ]; then + /bin/echo "Running self tests" + muorb test + MUORB_TEST_STATUS=$? + if [ $MUORB_TEST_STATUS -ne 0 ]; then + /bin/echo "muorb test failed" + shutdown + exit 0 + else + /bin/echo "muorb test passed" + fi +fi + muorb start diff --git a/src/modules/muorb/apps/CMakeLists.txt b/src/modules/muorb/apps/CMakeLists.txt index c6166b003e..010c8d3212 100644 --- a/src/modules/muorb/apps/CMakeLists.txt +++ b/src/modules/muorb/apps/CMakeLists.txt @@ -36,7 +36,10 @@ px4_add_module( MAIN muorb COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable + INCLUDES + ../test SRCS uORBAppsProtobufChannel.cpp muorb_main.cpp + ../test/MUORBTest.cpp ) diff --git a/src/modules/muorb/apps/muorb_main.cpp b/src/modules/muorb/apps/muorb_main.cpp index cc5ce657a6..0085bbd3ac 100644 --- a/src/modules/muorb/apps/muorb_main.cpp +++ b/src/modules/muorb/apps/muorb_main.cpp @@ -38,7 +38,7 @@ extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } static void usage() { - PX4_INFO("Usage: muorb 'start', 'stop', 'status'"); + PX4_INFO("Usage: muorb 'start', 'test', 'stop', 'status'"); } static bool enable_debug = false; @@ -53,19 +53,15 @@ muorb_main(int argc, char *argv[]) // TODO: Add an optional start parameter to control debug messages if (!strcmp(argv[1], "start")) { - if (uORB::AppsProtobufChannel::isInstance()) { - PX4_WARN("muorb already started"); + // Register the protobuf channel with UORB. + uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); - } else { - // Register the protobuf channel with UORB. - uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); + if (channel && channel->Initialize(enable_debug)) { return OK; } - if (channel) { - if (channel->Initialize(enable_debug)) { - return OK; - } - } - } + } else if (!strcmp(argv[1], "test")) { + uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); + + if (channel && channel->Initialize(enable_debug) && channel->Test()) { return OK; } } else if (!strcmp(argv[1], "stop")) { if (uORB::AppsProtobufChannel::isInstance() == false) { diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp index 8327e53c28..110d3832d6 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp @@ -36,6 +36,8 @@ #include "fc_sensor.h" +bool uORB::AppsProtobufChannel::test_flag = false; + // Initialize the static members uORB::AppsProtobufChannel *uORB::AppsProtobufChannel::_InstancePtr = nullptr; @@ -49,6 +51,24 @@ void uORB::AppsProtobufChannel::ReceiveCallback(const char *topic, } else if (strcmp(topic, "slpi_error") == 0) { PX4_ERR("SLPI: %s", (const char *) data); + } else if (IS_MUORB_TEST(topic)) { + // Validate the test data received + bool test_passed = true; + + if (length_in_bytes != MUORB_TEST_DATA_LEN) { + test_passed = false; + + } else { + for (uint32_t i = 0; i < length_in_bytes; i++) { + if ((uint8_t) i != data[i]) { + test_passed = false; + break; + } + } + } + + if (test_passed) { test_flag = true; } + } else { PX4_INFO("Got received data callback for topic %s", topic); } @@ -57,16 +77,86 @@ void uORB::AppsProtobufChannel::ReceiveCallback(const char *topic, void uORB::AppsProtobufChannel::AdvertiseCallback(const char *topic) { PX4_INFO("Got advertisement callback for topic %s", topic); + + if (IS_MUORB_TEST(topic)) { test_flag = true; } } void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic) { PX4_INFO("Got subscription callback for topic %s", topic); + + if (IS_MUORB_TEST(topic)) { test_flag = true; } } void uORB::AppsProtobufChannel::UnsubscribeCallback(const char *topic) { PX4_INFO("Got remove subscription callback for topic %s", topic); + + if (IS_MUORB_TEST(topic)) { test_flag = true; } +} + +bool uORB::AppsProtobufChannel::Test(MUORBTestType test_type) +{ + int rc = -1; + int timeout = 10; + + uint8_t test_data[MUORB_TEST_DATA_LEN]; + + for (uint8_t i = 0; i < MUORB_TEST_DATA_LEN; i++) { + test_data[i] = i; + }; + + test_flag = false; + + switch (test_type) { + case ADVERTISE_TEST_TYPE: + rc = fc_sensor_advertise(muorb_test_topic_name); + break; + + case SUBSCRIBE_TEST_TYPE: + rc = fc_sensor_subscribe(muorb_test_topic_name); + break; + + case TOPIC_TEST_TYPE: + rc = fc_sensor_send_data(muorb_test_topic_name, test_data, MUORB_TEST_DATA_LEN); + break; + + case UNSUBSCRIBE_TEST_TYPE: + rc = fc_sensor_unsubscribe(muorb_test_topic_name); + break; + + default: + break; + } + + // non zero return code means test failed + if (rc) { return false; } + + // Wait for test acknowledgement from DSP + while ((! test_flag) && (timeout--)) { + usleep(10000); + } + + if (timeout == -1) { + PX4_ERR("Test timed out waiting for response"); + return false; + } + + return true; +} + +bool uORB::AppsProtobufChannel::Test() +{ + if (! Test(ADVERTISE_TEST_TYPE)) { return false; } + + if (! Test(SUBSCRIBE_TEST_TYPE)) { return false; } + + if (! Test(TOPIC_TEST_TYPE)) { return false; } + + if (! Test(UNSUBSCRIBE_TEST_TYPE)) { return false; } + + PX4_INFO("muorb test passed"); + return true; } bool uORB::AppsProtobufChannel::Initialize(bool enable_debug) @@ -76,7 +166,7 @@ bool uORB::AppsProtobufChannel::Initialize(bool enable_debug) }; if (fc_sensor_initialize(enable_debug, &cb) != 0) { - PX4_ERR("Error calling the muorb protobuf initalize method"); + if (enable_debug) { PX4_INFO("Warning: muorb protobuf initalize method failed"); } } else { PX4_INFO("muorb protobuf initalize method succeeded"); diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp index e93eb38783..f320b074e5 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp @@ -36,6 +36,7 @@ #include #include +#include "MUORBTest.hpp" #include namespace uORB @@ -68,6 +69,8 @@ public: bool Initialize(bool enable_debug); + bool Test(); + private: // data members static uORB::AppsProtobufChannel *_InstancePtr; @@ -75,6 +78,10 @@ private://class members. /// constructor. AppsProtobufChannel() {}; + bool Test(MUORBTestType test_type); + + static bool test_flag; + static void ReceiveCallback(const char *topic, const uint8_t *data, uint32_t length_in_bytes); diff --git a/src/modules/muorb/slpi/CMakeLists.txt b/src/modules/muorb/slpi/CMakeLists.txt index 951b273ec6..41d9746ab4 100644 --- a/src/modules/muorb/slpi/CMakeLists.txt +++ b/src/modules/muorb/slpi/CMakeLists.txt @@ -33,5 +33,6 @@ px4_add_library(modules__muorb__slpi uORBProtobufChannel.cpp + ../test/MUORBTest.cpp ) -#target_include_directories(modules__muorb__slpi PRIVATE ${PX4_SOURCE_DIR}/src/modules/uORB) +target_include_directories(modules__muorb__slpi PRIVATE ../test) diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index ed65ca1a82..9fc1bdb4e5 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -31,34 +31,128 @@ * ****************************************************************************/ #include "uORBProtobufChannel.hpp" +#include "MUORBTest.hpp" +#include -// TODO: Move this out of here once we have it placed properly +#include +#include + +// TODO: Move this out of here once we have px4-log functionality extern "C" void HAP_debug(const char *msg, int level, const char *filename, int line); +// Definition of test to run when in muorb test mode +static MUORBTestType test_to_run; + +fc_func_ptrs muorb_func_ptrs; + +static void test_runner(void *test) +{ + HAP_debug("test_runner called", 1, muorb_test_topic_name, 0); + + switch (*((MUORBTestType *) test)) { + case ADVERTISE_TEST_TYPE: + (void) muorb_func_ptrs.advertise_func_ptr(muorb_test_topic_name); + break; + + case SUBSCRIBE_TEST_TYPE: + (void) muorb_func_ptrs.subscribe_func_ptr(muorb_test_topic_name); + break; + + case UNSUBSCRIBE_TEST_TYPE: + (void) muorb_func_ptrs.unsubscribe_func_ptr(muorb_test_topic_name); + break; + + case TOPIC_TEST_TYPE: { + uint8_t data[MUORB_TEST_DATA_LEN]; + + for (uint8_t i = 0; i < MUORB_TEST_DATA_LEN; i++) { data[i] = i; } + + (void) muorb_func_ptrs.topic_data_func_ptr(muorb_test_topic_name, data, MUORB_TEST_DATA_LEN); + } + + default: + break; + } + + qurt_thread_exit(0); +} + int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) { - HAP_debug("Hello, world!", 1, "test", 0); + // These function pointers will only be non-null on the first call + // so they must be saved off here + if (func_ptrs != nullptr) { muorb_func_ptrs = *func_ptrs; } + + HAP_debug("px4muorb_orb_initialize called", 1, "init", 0); return 0; } +#define TEST_STACK_SIZE 8192 +char stack[TEST_STACK_SIZE]; + +void run_test(MUORBTestType test) +{ + qurt_thread_t tid; + qurt_thread_attr_t attr; + + qurt_thread_attr_init(&attr); + qurt_thread_attr_set_stack_addr(&attr, stack); + qurt_thread_attr_set_stack_size(&attr, TEST_STACK_SIZE); + test_to_run = test; + (void) qurt_thread_create(&tid, &attr, &test_runner, (void *) &test_to_run); +} + int px4muorb_topic_advertised(const char *topic_name) { + if (IS_MUORB_TEST(topic_name)) { run_test(ADVERTISE_TEST_TYPE); } + + HAP_debug("px4muorb_topic_advertised called", 1, topic_name, 0); + return 0; } int px4muorb_add_subscriber(const char *topic_name) { + if (IS_MUORB_TEST(topic_name)) { run_test(SUBSCRIBE_TEST_TYPE); } + + HAP_debug("px4muorb_add_subscriber called", 1, topic_name, 0); + return 0; } int px4muorb_remove_subscriber(const char *topic_name) { + if (IS_MUORB_TEST(topic_name)) { run_test(UNSUBSCRIBE_TEST_TYPE); } + + HAP_debug("px4muorb_remove_subscriber called", 1, topic_name, 0); + return 0; } int px4muorb_send_topic_data(const char *topic_name, const uint8_t *data, int data_len_in_bytes) { + if (IS_MUORB_TEST(topic_name)) { + // Validate the test data received + bool test_passed = true; + + if (data_len_in_bytes != MUORB_TEST_DATA_LEN) { + test_passed = false; + + } else { + for (int i = 0; i < data_len_in_bytes; i++) { + if ((uint8_t) i != data[i]) { + test_passed = false; + break; + } + } + } + + if (test_passed) { run_test(TOPIC_TEST_TYPE); } + } + + HAP_debug("px4muorb_send_topic_data called", 1, topic_name, 0); + return 0; } diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.hpp b/src/modules/muorb/slpi/uORBProtobufChannel.hpp index 104d478bf0..d39f51d5ef 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.hpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.hpp @@ -36,8 +36,6 @@ #include #include -//#include - // TODO: This has to be defined in the slpi_proc build and in the PX4 build. // Make it accessible from one file to both builds. typedef struct { diff --git a/src/modules/muorb/test/MUORBTest.cpp b/src/modules/muorb/test/MUORBTest.cpp new file mode 100644 index 0000000000..61921cedca --- /dev/null +++ b/src/modules/muorb/test/MUORBTest.cpp @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +// Definition of common test name between DSP and CPU +char muorb_test_topic_name[] = "muorb_test"; diff --git a/src/modules/muorb/test/MUORBTest.hpp b/src/modules/muorb/test/MUORBTest.hpp new file mode 100644 index 0000000000..83da6a8fb3 --- /dev/null +++ b/src/modules/muorb/test/MUORBTest.hpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +// These are the test types for each of the operations that can happen with muorb +typedef enum { + ADVERTISE_TEST_TYPE, + SUBSCRIBE_TEST_TYPE, + TOPIC_TEST_TYPE, + UNSUBSCRIBE_TEST_TYPE +} MUORBTestType; + +// Common test name between DSP and CPU +extern char muorb_test_topic_name[]; + +// Check if topic is muorb test marker +#define IS_MUORB_TEST(x) (strcmp(x, muorb_test_topic_name) == 0) + +#define MUORB_TEST_DATA_LEN 8