Added a self test feature to the VOXL 2 muorb module (#20307)

* Removed exit after dsp signature generation.
* First full test suite
* Cleaned up the muorb tests
* Improved VOXL 2 self test trigger in startup file
* Removed unneeded include file

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"
-    fi
         exit 0
     fi
+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

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

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
 	)

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");
-
-		} else {
 		// Register the protobuf channel with UORB.
 		uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance();
 
-			if (channel) {
+		if (channel && channel->Initialize(enable_debug)) { return OK; }
-				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) {

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");

src/modules/muorb/apps/uORBAppsProtobufChannel.hpp

@@ -36,6 +36,7 @@
 
 #include <stdint.h>
 #include <string>
+#include "MUORBTest.hpp"
 #include <px4_platform_common/log.h>
 
 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);

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)

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -31,34 +31,128 @@
  *
  ****************************************************************************/
 #include "uORBProtobufChannel.hpp"
+#include "MUORBTest.hpp"
+#include <string>
 
-// TODO: Move this out of here once we have it placed properly
+#include <qurt.h>
+#include <qurt_thread.h>
+
+// 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;
 }

src/modules/muorb/slpi/uORBProtobufChannel.hpp

@@ -36,8 +36,6 @@
 #include <stdint.h>
 #include <stdio.h>
 
-//#include <unistd.h>
-
 // 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 {

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";

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