forked from Archive/PX4-Autopilot
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
This commit is contained in:
Eric Katzfey
GitHub
parent
7bc79b491f
commit
a5bfc3fbc2
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue