mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
fix(mc_nn_control): guard the model, the observations and the output (#28504)
* fix(mc_nn_control): reject a model the module cannot feed The documentation tells users to swap the model array in control_net.cpp, and the module then wrote 15 floats into whatever input tensor came back and read 4 outputs from whatever output tensor came back, with no check of either. A model with a different input width would write past the tensor in the arena with no diagnostic. Reported in #28417. Check the schema version before the interpreter is built on the model, then the number of input and output tensors and the element count and type of each, and refuse to start with the reason in the log when they do not match. The checks live in a header without module dependencies so they can be unit tested, and the operator registration moves to a header so a test can load the shipped model through the real interpreter. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> * fix(mc_nn_control): guard the observations and the network output The network was fed whatever the position and attitude topics last held, with no check of the validity flags, the age or finiteness, and its output went to the motors with any non finite channel mapped to a stopped motor while the other three kept thrusting. Reported in #28417. Check the observations on every cycle, in the mode or not: the position validity flags, the age of position, attitude and angular velocity against the same limits the raptor module uses, and finiteness. The network is not run on observations that fail, and a non finite output is not sent. In both cases the last command of this session is repeated, which is what the mixer would apply anyway, and the arming check reply says the mode cannot run, so the commander leaves it through its own fallback within one of its external check intervals. The repeat keeps the actuator topic and the lockstep simulators going, which wait for an output on every step. The first fault of an outage is reported once. A non finite output is reported once and the network is not trusted again until the mode has been left. The module runs on angular velocity updates, so a lost angular velocity stream would never have woken it. It is now a scheduled work item that re-arms a delayed run on every cycle, which keeps it registering, checking and answering the commander when the updates stop. The trajectory setpoint starts unset and is reset to the current position when the mode is entered without one. Before, the member was never initialized and the reset only happened when a position update landed on the same cycle as the first angular velocity update. In SIH, a GPS loss while flying the neural mode produced the fault event the same instant the position validity dropped, inference stopped, and the commander was in Descend within its next check. A gyro loss on a real time build produced the fault event 20 ms after the last sample and the commander was in failsafe 150 ms later. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> * test(mc_nn_control): cover the model, observation and output checks Eleven cases on the checks the module runs before trusting its model, its observations and its output: every position validity flag, each staleness limit at its boundary and pinned to the raptor module's values, never set and future stamps, non finite values in each observation, the order in which faults are reported, every channel of the output, and every way a model can fail the layout check. One more loads the shipped model through the real interpreter and runs the module's layout check on it, so a swapped model array the module cannot feed fails here before it fails on a vehicle. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> * fix(mc_nn_control): start the observation caches empty The three cached observations were plain members, so the first observation check read whatever the memory held before any of the topics had published, and a stream that had not arrived could pass that first check by chance. They start empty now, which the first check reports as an invalid position. A functional test builds the module in storage filled beforehand and runs the check with nothing published. Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> * ci(tests): run every mc_nn_control test in tests_neural The default filter of the target only matched the first test of the module. It now selects every test of the module, and it is quoted so the shell does not read the alternation as a pipe. Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> --------- Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>
This commit is contained in:
1 parent
b12d17f632
commit
4da3cff386
10 files changed
+854
-41
No files matched your search
@@ -475,7 +475,7 @@ tests_vtest_moving:
|
||||
|
||||
# This target builds the neural configuration, the only one that enables mc_nn_control.
|
||||
tests_neural:
|
||||
$(eval override CMAKE_ARGS += -DTESTFILTER=$(if $(TESTFILTER),$(TESTFILTER),RescaleAction))
|
||||
$(eval override CMAKE_ARGS += -DTESTFILTER="$(if $(TESTFILTER),$(TESTFILTER),RescaleAction|NnControl)")
|
||||
$(eval override CMAKE_ARGS += -DCMAKE_TESTING=ON)
|
||||
$(eval ARGS += test_results)
|
||||
$(eval ASAN_OPTIONS += color=always:check_initialization_order=1:detect_stack_use_after_return=1)
|
||||
|
||||
@@ -40,6 +40,9 @@ You will then have to modify the `control_net.hpp` and `control_net.cpp` to incl
|
||||
- Take the size of the network in the bottom of the `.cc` file and replace the size in `control_net.hpp`.
|
||||
- Take the data in the model array in the `cc` file, and replace the ones in `control_net.cpp`.
|
||||
|
||||
The module checks the model when it starts: the schema version, exactly one input of 15 floats and exactly one output of 4 floats.
|
||||
A model that does not match is rejected with the reason in the log and the module does not start.
|
||||
|
||||
You are now ready to run your own network.
|
||||
|
||||
## Code Explanation
|
||||
|
||||
@@ -43,6 +43,8 @@ px4_add_module(
|
||||
mc_nn_control.cpp
|
||||
mc_nn_control.hpp
|
||||
actions_rescale.hpp
|
||||
nn_control_checks.hpp
|
||||
nn_control_model.hpp
|
||||
control_net.cpp
|
||||
control_net.hpp
|
||||
MODULE_CONFIG
|
||||
@@ -57,3 +59,11 @@ target_include_directories(mc_nn_control PRIVATE
|
||||
${CMAKE_SOURCE_DIR}/src/lib/tensorflow_lite_micro)
|
||||
|
||||
px4_add_unit_gtest(SRC RescaleActionTest.cpp)
|
||||
px4_add_unit_gtest(SRC NnControlChecksTest.cpp)
|
||||
px4_add_unit_gtest(SRC NnControlModelTest.cpp
|
||||
EXTRA_SRCS control_net.cpp
|
||||
LINKLIBS tensorflow_lite_micro
|
||||
INCLUDES ${CMAKE_SOURCE_DIR}/src/lib/tensorflow_lite_micro)
|
||||
px4_add_functional_gtest(SRC NnControlStartupTest.cpp
|
||||
LINKLIBS mc_nn_control tensorflow_lite_micro
|
||||
INCLUDES ${CMAKE_SOURCE_DIR}/src/lib/tensorflow_lite_micro)
|
||||
@@ -0,0 +1,183 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file NnControlChecksTest.cpp
|
||||
* Unit tests for the checks the neural controller runs before it trusts its model,
|
||||
* its observations and its output.
|
||||
*
|
||||
* to run, on a config that enables the module:
|
||||
* cmake -DCMAKE_TESTING=ON build/px4_sitl_neural
|
||||
* ninja -C build/px4_sitl_neural unit-NnControlChecks
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "nn_control_checks.hpp"
|
||||
|
||||
using namespace nn_control;
|
||||
|
||||
static ObservationSnapshot good(uint64_t now)
|
||||
{
|
||||
ObservationSnapshot o{};
|
||||
o.position_timestamp = now - 1000;
|
||||
o.xy_valid = o.z_valid = o.v_xy_valid = o.v_z_valid = true;
|
||||
o.position[0] = 1.f;
|
||||
o.position[1] = -2.f;
|
||||
o.position[2] = -3.f;
|
||||
o.velocity[0] = 0.1f;
|
||||
o.velocity[1] = 0.f;
|
||||
o.velocity[2] = -0.2f;
|
||||
o.attitude_timestamp = now - 1000;
|
||||
o.q[0] = 1.f;
|
||||
o.angular_velocity_timestamp = now - 500;
|
||||
o.angular_velocity[1] = 0.01f;
|
||||
return o;
|
||||
}
|
||||
|
||||
static constexpr uint64_t kNow = 10000000;
|
||||
|
||||
TEST(NnControlChecksTest, staleLimitsAreTheRaptorModulesLimits)
|
||||
{
|
||||
// the contract, not a restatement of the constants
|
||||
EXPECT_EQ(kMaxLocalPositionAge, 100000u);
|
||||
EXPECT_EQ(kMaxAttitudeAge, 50000u);
|
||||
EXPECT_EQ(kMaxAngularVelocityAge, 10000u);
|
||||
EXPECT_EQ(kAngularVelocityWatchdog, 20000u);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, freshObservationsPass)
|
||||
{
|
||||
EXPECT_EQ(check_observations(good(kNow), kNow), ObservationFault::None);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, everyInvalidPositionFlagIsCaught)
|
||||
{
|
||||
for (int flag = 0; flag < 4; flag++) {
|
||||
ObservationSnapshot o = good(kNow);
|
||||
(flag == 0 ? o.xy_valid : flag == 1 ? o.z_valid : flag == 2 ? o.v_xy_valid : o.v_z_valid) = false;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::PositionInvalid) << "flag " << flag;
|
||||
}
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, staleObservationsAreCaughtAtTheirOwnLimits)
|
||||
{
|
||||
ObservationSnapshot o = good(kNow);
|
||||
o.position_timestamp = kNow - kMaxLocalPositionAge;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::None);
|
||||
o.position_timestamp = kNow - kMaxLocalPositionAge - 1;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::PositionStale);
|
||||
|
||||
o = good(kNow);
|
||||
o.attitude_timestamp = kNow - kMaxAttitudeAge - 1;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::AttitudeStale);
|
||||
|
||||
o = good(kNow);
|
||||
o.angular_velocity_timestamp = kNow - kMaxAngularVelocityAge - 1;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::AngularVelocityStale);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, neverSetObservationsAreStale)
|
||||
{
|
||||
ObservationSnapshot o = good(kNow);
|
||||
o.position_timestamp = 0;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::PositionStale);
|
||||
|
||||
o = good(kNow);
|
||||
o.attitude_timestamp = 0;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::AttitudeStale);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, observationsFromTheFutureAreNotStale)
|
||||
{
|
||||
ObservationSnapshot o = good(kNow);
|
||||
o.position_timestamp = kNow + 5000;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::None);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, nonFiniteObservationsAreCaught)
|
||||
{
|
||||
ObservationSnapshot o = good(kNow);
|
||||
o.position[2] = NAN;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::PositionNotFinite);
|
||||
|
||||
o = good(kNow);
|
||||
o.velocity[0] = INFINITY;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::PositionNotFinite);
|
||||
|
||||
o = good(kNow);
|
||||
o.q[3] = NAN;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::AttitudeNotFinite);
|
||||
|
||||
o = good(kNow);
|
||||
o.angular_velocity[2] = -INFINITY;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::AngularVelocityNotFinite);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, positionProblemsAreReportedBeforeAttitudeProblems)
|
||||
{
|
||||
ObservationSnapshot o = good(kNow);
|
||||
o.xy_valid = false;
|
||||
o.attitude_timestamp = 0;
|
||||
EXPECT_EQ(check_observations(o, kNow), ObservationFault::PositionInvalid);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, outputsHaveToBeFiniteInEveryChannel)
|
||||
{
|
||||
float out[kOutputSize] = {0.5f, 0.5f, 0.5f, 0.5f};
|
||||
EXPECT_TRUE(outputs_finite(out, kOutputSize));
|
||||
|
||||
for (int i = 0; i < kOutputSize; i++) {
|
||||
float bad[kOutputSize] = {0.5f, 0.5f, 0.5f, 0.5f};
|
||||
bad[i] = (i % 2 == 0) ? NAN : INFINITY;
|
||||
EXPECT_FALSE(outputs_finite(bad, kOutputSize)) << "channel " << i;
|
||||
}
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, theExpectedModelLayoutIsAccepted)
|
||||
{
|
||||
EXPECT_EQ(model_layout_problem(3, 3, 1, kInputSize, true, 1, kOutputSize, true), nullptr);
|
||||
}
|
||||
|
||||
TEST(NnControlChecksTest, everyModelLayoutMismatchIsNamed)
|
||||
{
|
||||
EXPECT_STREQ(model_layout_problem(2, 3, 1, kInputSize, true, 1, kOutputSize, true), "schema version");
|
||||
EXPECT_STREQ(model_layout_problem(3, 3, 2, kInputSize, true, 1, kOutputSize, true), "input tensors, expected one");
|
||||
EXPECT_STREQ(model_layout_problem(3, 3, 1, kInputSize, true, 2, kOutputSize, true), "output tensors, expected one");
|
||||
EXPECT_STREQ(model_layout_problem(3, 3, 1, 12, true, 1, kOutputSize, true), "input size, expected 15 elements");
|
||||
EXPECT_STREQ(model_layout_problem(3, 3, 1, kInputSize, false, 1, kOutputSize, true), "input type, expected float32");
|
||||
EXPECT_STREQ(model_layout_problem(3, 3, 1, kInputSize, true, 1, 6, true), "output size, expected 4 elements");
|
||||
EXPECT_STREQ(model_layout_problem(3, 3, 1, kInputSize, true, 1, kOutputSize, false), "output type, expected float32");
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file NnControlModelTest.cpp
|
||||
* Loads the shipped network through the real interpreter and runs the layout
|
||||
* check the module runs at start on it, so a swapped model array that the
|
||||
* module cannot feed fails here before it fails on a vehicle.
|
||||
*
|
||||
* to run, on a config that enables the module:
|
||||
* cmake -DCMAKE_TESTING=ON build/px4_sitl_neural
|
||||
* ninja -C build/px4_sitl_neural unit-NnControlModel
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <tflite_micro/tensorflow/lite/micro/micro_interpreter.h>
|
||||
#include <tflite_micro/tensorflow/lite/schema/schema_generated.h>
|
||||
|
||||
#include "control_net.hpp"
|
||||
#include "nn_control_checks.hpp"
|
||||
#include "nn_control_model.hpp"
|
||||
|
||||
static int element_count(const TfLiteTensor *tensor)
|
||||
{
|
||||
int count = 1;
|
||||
|
||||
for (int i = 0; i < tensor->dims->size; i++) {
|
||||
count *= tensor->dims->data[i];
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
TEST(NnControlModelTest, theShippedModelPassesTheLayoutCheck)
|
||||
{
|
||||
const tflite::Model *model = ::tflite::GetModel(control_net_tflite);
|
||||
ASSERT_NE(model, nullptr);
|
||||
EXPECT_EQ(model->version(), TFLITE_SCHEMA_VERSION);
|
||||
|
||||
static nn_control::OpResolver resolver;
|
||||
ASSERT_EQ(nn_control::register_ops(resolver), kTfLiteOk);
|
||||
|
||||
constexpr int kArenaSize = 10 * 1024;
|
||||
static uint8_t arena[kArenaSize];
|
||||
tflite::MicroInterpreter interpreter(model, resolver, arena, kArenaSize);
|
||||
ASSERT_EQ(interpreter.AllocateTensors(), kTfLiteOk);
|
||||
|
||||
const TfLiteTensor *input = interpreter.input(0);
|
||||
const TfLiteTensor *output = interpreter.output(0);
|
||||
ASSERT_NE(input, nullptr);
|
||||
ASSERT_NE(output, nullptr);
|
||||
|
||||
EXPECT_EQ(interpreter.inputs_size(), 1u);
|
||||
EXPECT_EQ(interpreter.outputs_size(), 1u);
|
||||
EXPECT_EQ(element_count(input), nn_control::kInputSize);
|
||||
EXPECT_EQ(element_count(output), nn_control::kOutputSize);
|
||||
|
||||
const char *problem = nn_control::model_layout_problem(model->version(), TFLITE_SCHEMA_VERSION,
|
||||
(int)interpreter.inputs_size(), element_count(input), input->type == kTfLiteFloat32,
|
||||
(int)interpreter.outputs_size(), element_count(output), output->type == kTfLiteFloat32);
|
||||
EXPECT_EQ(problem, nullptr) << problem;
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* @file NnControlStartupTest.cpp
|
||||
*
|
||||
* The observation check runs on every cycle from the first one, before any of the
|
||||
* three observation topics has necessarily published. The caches it reads start
|
||||
* empty, so that first check reports a missing position instead of reading whatever
|
||||
* the memory held.
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cstring>
|
||||
#include <new>
|
||||
|
||||
#include "mc_nn_control.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <hrt_work.h>
|
||||
#include <parameters/param.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
|
||||
|
||||
class NnControlStartupTestPeer
|
||||
{
|
||||
public:
|
||||
static nn_control::ObservationFault checkObservations(MulticopterNeuralNetworkControl &module)
|
||||
{
|
||||
module.CheckObservations();
|
||||
return module._observation_fault;
|
||||
}
|
||||
};
|
||||
|
||||
class NnControlStartupTestEnvironment : public ::testing::Environment
|
||||
{
|
||||
public:
|
||||
void TearDown() override
|
||||
{
|
||||
px4::WorkQueueManagerStop();
|
||||
}
|
||||
};
|
||||
|
||||
static const auto *global_env = ::testing::AddGlobalTestEnvironment(new NnControlStartupTestEnvironment());
|
||||
|
||||
class NnControlStartupTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
static bool work_queue_started = false;
|
||||
|
||||
if (!work_queue_started) {
|
||||
// The module is a scheduled work item, so its construction needs the work
|
||||
// queue manager, and the manager's timer lock needs hrt_init() first
|
||||
hrt_init();
|
||||
ASSERT_EQ(px4::WorkQueueManagerStart(), 0);
|
||||
work_queue_started = true;
|
||||
}
|
||||
|
||||
param_control_autosave(false);
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(NnControlStartupTest, nothingPublishedIsReportedAsPositionInvalid)
|
||||
{
|
||||
// The module as it is right after construction, before init() and before any of
|
||||
// the observation topics has published. Its storage is filled beforehand so the
|
||||
// outcome does not depend on what the stack happened to hold, which is how the
|
||||
// caches were read before they were initialised.
|
||||
alignas(MulticopterNeuralNetworkControl) static unsigned char storage[sizeof(MulticopterNeuralNetworkControl)];
|
||||
memset(storage, 0xff, sizeof(storage));
|
||||
MulticopterNeuralNetworkControl *module = new (storage) MulticopterNeuralNetworkControl();
|
||||
|
||||
EXPECT_EQ(NnControlStartupTestPeer::checkObservations(*module), nn_control::ObservationFault::PositionInvalid);
|
||||
|
||||
module->~MulticopterNeuralNetworkControl();
|
||||
}
|
||||
@@ -52,25 +52,19 @@ ModuleBase::Descriptor MulticopterNeuralNetworkControl::desc{task_spawn, custom_
|
||||
|
||||
namespace
|
||||
{
|
||||
// This number should be the number of operations in the model, like tanh and fully connected
|
||||
using NNControlOpResolver = tflite::MicroMutableOpResolver<3>;
|
||||
|
||||
TfLiteStatus RegisterOps(NNControlOpResolver &op_resolver)
|
||||
{
|
||||
// Add the operations to you need to the op_resolver
|
||||
TF_LITE_ENSURE_STATUS(op_resolver.AddFullyConnected());
|
||||
TF_LITE_ENSURE_STATUS(op_resolver.AddRelu());
|
||||
TF_LITE_ENSURE_STATUS(op_resolver.AddAdd());
|
||||
return kTfLiteOk;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
MulticopterNeuralNetworkControl::MulticopterNeuralNetworkControl() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
|
||||
// Unset until the mode is entered or a setpoint arrives, so the network is
|
||||
// never pointed at whatever happened to be in memory
|
||||
_trajectory_setpoint.position[0] = NAN;
|
||||
_trajectory_setpoint.position[1] = NAN;
|
||||
_trajectory_setpoint.position[2] = NAN;
|
||||
_trajectory_setpoint.yaw = NAN;
|
||||
}
|
||||
|
||||
MulticopterNeuralNetworkControl::~MulticopterNeuralNetworkControl()
|
||||
@@ -93,6 +87,10 @@ bool MulticopterNeuralNetworkControl::init()
|
||||
updateParams();
|
||||
UpdateMotorLimits();
|
||||
|
||||
// The first run registers the mode and arms the watchdog, so it must not wait
|
||||
// for a sensor that may never publish
|
||||
ScheduleNow();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -146,6 +144,35 @@ void MulticopterNeuralNetworkControl::ReportInvalidLimits()
|
||||
(int32_t)_param_max_rpm.get(), (int32_t)_param_min_rpm.get(), _param_thrust_coeff.get());
|
||||
}
|
||||
|
||||
void MulticopterNeuralNetworkControl::CheckObservations()
|
||||
{
|
||||
nn_control::ObservationSnapshot snapshot{};
|
||||
snapshot.position_timestamp = _position.timestamp;
|
||||
snapshot.xy_valid = _position.xy_valid;
|
||||
snapshot.z_valid = _position.z_valid;
|
||||
snapshot.v_xy_valid = _position.v_xy_valid;
|
||||
snapshot.v_z_valid = _position.v_z_valid;
|
||||
snapshot.position[0] = _position.x;
|
||||
snapshot.position[1] = _position.y;
|
||||
snapshot.position[2] = _position.z;
|
||||
snapshot.velocity[0] = _position.vx;
|
||||
snapshot.velocity[1] = _position.vy;
|
||||
snapshot.velocity[2] = _position.vz;
|
||||
snapshot.attitude_timestamp = _attitude.timestamp;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
snapshot.q[i] = _attitude.q[i];
|
||||
}
|
||||
|
||||
snapshot.angular_velocity_timestamp = _angular_velocity.timestamp;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
snapshot.angular_velocity[i] = _angular_velocity.xyz[i];
|
||||
}
|
||||
|
||||
_observation_fault = nn_control::check_observations(snapshot, hrt_absolute_time());
|
||||
}
|
||||
|
||||
void MulticopterNeuralNetworkControl::ReportActionRange()
|
||||
{
|
||||
// Say which part of the action range this motor can reproduce, so a mismatch
|
||||
@@ -176,10 +203,17 @@ int MulticopterNeuralNetworkControl::InitializeNetwork()
|
||||
// Initialize the neural network
|
||||
const tflite::Model *control_model = ::tflite::GetModel(control_net_tflite);
|
||||
|
||||
// Set up the interpreter
|
||||
static NNControlOpResolver resolver;
|
||||
// Checked before the interpreter touches it, a model from another schema
|
||||
// cannot be parsed
|
||||
if (control_model->version() != TFLITE_SCHEMA_VERSION) {
|
||||
PX4_ERR("model rejected: schema version %d, expected %d", (int)control_model->version(), TFLITE_SCHEMA_VERSION);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (RegisterOps(resolver) != kTfLiteOk) {
|
||||
// Set up the interpreter
|
||||
static nn_control::OpResolver resolver;
|
||||
|
||||
if (nn_control::register_ops(resolver) != kTfLiteOk) {
|
||||
PX4_ERR("Failed to register ops");
|
||||
return -1;
|
||||
}
|
||||
@@ -204,14 +238,39 @@ int MulticopterNeuralNetworkControl::InitializeNetwork()
|
||||
}
|
||||
|
||||
_input_tensor = _interpreter->input(0);
|
||||
TfLiteTensor *output_tensor = _interpreter->output(0);
|
||||
|
||||
if (_input_tensor == nullptr) {
|
||||
PX4_ERR("Input tensor is null");
|
||||
if ((_input_tensor == nullptr) || (output_tensor == nullptr)) {
|
||||
PX4_ERR("model has no input or output tensor");
|
||||
delete _interpreter;
|
||||
_interpreter = nullptr;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// The documentation tells users to swap the model array, so check that what was
|
||||
// loaded is the shape this module feeds and reads
|
||||
auto element_count = [](const TfLiteTensor * tensor) {
|
||||
int count = 1;
|
||||
|
||||
for (int i = 0; i < tensor->dims->size; i++) {
|
||||
count *= tensor->dims->data[i];
|
||||
}
|
||||
|
||||
return count;
|
||||
};
|
||||
|
||||
const char *problem = nn_control::model_layout_problem(control_model->version(), TFLITE_SCHEMA_VERSION,
|
||||
(int)_interpreter->inputs_size(), element_count(_input_tensor), _input_tensor->type == kTfLiteFloat32,
|
||||
(int)_interpreter->outputs_size(), element_count(output_tensor), output_tensor->type == kTfLiteFloat32);
|
||||
|
||||
if (problem != nullptr) {
|
||||
PX4_ERR("model rejected: %s", problem);
|
||||
delete _interpreter;
|
||||
_interpreter = nullptr;
|
||||
_input_tensor = nullptr;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
@@ -272,7 +331,9 @@ void MulticopterNeuralNetworkControl::ReplyToArmingCheck(int8 request_id)
|
||||
arming_check_reply.registration_id = _arming_check_id;
|
||||
arming_check_reply.health_component_index = arming_check_reply.HEALTH_COMPONENT_INDEX_NONE;
|
||||
arming_check_reply.num_events = 0;
|
||||
arming_check_reply.can_arm_and_run = _motor_limits_valid;
|
||||
arming_check_reply.can_arm_and_run = _motor_limits_valid
|
||||
&& (_observation_fault == nn_control::ObservationFault::None)
|
||||
&& !_output_fault;
|
||||
|
||||
// The reason is a standalone event, repeated while the limits stay invalid so
|
||||
// it is not lost on a link that came up later
|
||||
@@ -510,10 +571,22 @@ int MulticopterNeuralNetworkControl::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void MulticopterNeuralNetworkControl::HoldLastCommand()
|
||||
{
|
||||
// The mixer keeps the last command on its own, so repeating it changes nothing
|
||||
// at the motors. It keeps the actuator topic and the lockstep simulators, which
|
||||
// wait for an output every step, going while the commander leaves the mode.
|
||||
// Only a command from this session is repeated, never one from an earlier one.
|
||||
if (_have_last_command) {
|
||||
PublishOutput(_last_command);
|
||||
}
|
||||
}
|
||||
|
||||
void MulticopterNeuralNetworkControl::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_angular_velocity_sub.unregisterCallback();
|
||||
ScheduleClear();
|
||||
|
||||
if (_sent_mode_registration) {
|
||||
UnregisterNeuralFlightMode(_arming_check_id, _mode_id);
|
||||
@@ -523,6 +596,11 @@ void MulticopterNeuralNetworkControl::Run()
|
||||
return;
|
||||
}
|
||||
|
||||
// Runs are driven by angular velocity updates. This delayed run is what keeps the
|
||||
// module registering, checking the observations and answering the commander when
|
||||
// those updates stop, in the mode or not.
|
||||
ScheduleDelayed(nn_control::kAngularVelocityWatchdog);
|
||||
|
||||
// Register the flight mode with the commander
|
||||
if (!_sent_mode_registration) {
|
||||
RegisterNeuralFlightMode();
|
||||
@@ -556,6 +634,13 @@ void MulticopterNeuralNetworkControl::Run()
|
||||
if (!prev_use_neural && _use_neural) {
|
||||
ConfigureNeuralFlightMode(_mode_id);
|
||||
ReportActionRange();
|
||||
|
||||
// Without a setpoint of its own the mode holds the position it was entered at
|
||||
if (!PX4_ISFINITE(_trajectory_setpoint.position[0])
|
||||
|| !PX4_ISFINITE(_trajectory_setpoint.position[1])
|
||||
|| !PX4_ISFINITE(_trajectory_setpoint.position[2])) {
|
||||
reset_trajectory_setpoint(_position);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -566,8 +651,60 @@ void MulticopterNeuralNetworkControl::Run()
|
||||
UpdateMotorLimits();
|
||||
}
|
||||
|
||||
// The observations are refreshed and checked on every cycle, in the mode or not,
|
||||
// so the arming check reply always describes the current state
|
||||
const bool angular_velocity_updated = _angular_velocity_sub.update(&_angular_velocity);
|
||||
|
||||
if (_attitude_sub.updated()) {
|
||||
_attitude_sub.copy(&_attitude);
|
||||
}
|
||||
|
||||
const bool position_updated = _position_sub.updated();
|
||||
|
||||
if (position_updated) {
|
||||
_position_sub.copy(&_position);
|
||||
}
|
||||
|
||||
CheckObservations();
|
||||
|
||||
if (!_use_neural || !_mapping_valid) {
|
||||
// If the neural network flight mode is not enabled, do nothing
|
||||
// If the neural network flight mode is not enabled, do nothing. A network that
|
||||
// misbehaved is trusted again once the mode has been left.
|
||||
_output_fault = false;
|
||||
_reported_observation_fault = nn_control::ObservationFault::None;
|
||||
_have_last_command = false;
|
||||
perf_end(_loop_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
if (_observation_fault != nn_control::ObservationFault::None) {
|
||||
// Not run on observations that are missing, stale or not finite. The last
|
||||
// command of this session is repeated and the arming check reply has the
|
||||
// commander leave the mode. Checked here rather than on the angular velocity
|
||||
// update so that a lost angular velocity stream is reported too.
|
||||
if (_reported_observation_fault == nn_control::ObservationFault::None) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* The network is not run on observations that are missing, stale or not finite. The last
|
||||
* command is repeated and the mode reports itself unable to run so the commander leaves it.
|
||||
* Reason: 1 position invalid, 2 position stale, 3 position not finite, 4 attitude stale,
|
||||
* 5 attitude not finite, 6 angular velocity stale, 7 angular velocity not finite.
|
||||
*/
|
||||
events::send<uint8_t>(events::ID("mc_nn_control_observations_invalid"), events::Log::Error,
|
||||
"Neural control: observations invalid ({1}), not running the network",
|
||||
(uint8_t)_observation_fault);
|
||||
_reported_observation_fault = _observation_fault;
|
||||
}
|
||||
|
||||
HoldLastCommand();
|
||||
perf_end(_loop_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
_reported_observation_fault = nn_control::ObservationFault::None;
|
||||
|
||||
if (_output_fault) {
|
||||
HoldLastCommand();
|
||||
perf_end(_loop_perf);
|
||||
return;
|
||||
}
|
||||
@@ -575,23 +712,16 @@ void MulticopterNeuralNetworkControl::Run()
|
||||
int32_t start_time1 = GetTime();
|
||||
|
||||
// run controller on angular velocity updates
|
||||
if (_angular_velocity_sub.update(&_angular_velocity)) {
|
||||
if (angular_velocity_updated) {
|
||||
const float dt = math::constrain(((_angular_velocity.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f);
|
||||
_last_run = _angular_velocity.timestamp_sample;
|
||||
|
||||
if (_attitude_sub.updated()) {
|
||||
_attitude_sub.copy(&_attitude);
|
||||
}
|
||||
|
||||
if (_position_sub.updated()) {
|
||||
_position_sub.copy(&_position);
|
||||
|
||||
// If there is no position setpoint, use the position when switching mode as the setpoint
|
||||
if (!PX4_ISFINITE(_trajectory_setpoint.position[0])
|
||||
&& !PX4_ISFINITE(_trajectory_setpoint.position[1])
|
||||
&& !PX4_ISFINITE(_trajectory_setpoint.position[2])) {
|
||||
reset_trajectory_setpoint(_position);
|
||||
}
|
||||
// If there is no position setpoint, use the position when switching mode as the setpoint
|
||||
if (position_updated
|
||||
&& !PX4_ISFINITE(_trajectory_setpoint.position[0])
|
||||
&& !PX4_ISFINITE(_trajectory_setpoint.position[1])
|
||||
&& !PX4_ISFINITE(_trajectory_setpoint.position[2])) {
|
||||
reset_trajectory_setpoint(_position);
|
||||
}
|
||||
|
||||
if (_param_manual_control.get()) {
|
||||
@@ -638,9 +768,31 @@ void MulticopterNeuralNetworkControl::Run()
|
||||
return;
|
||||
}
|
||||
|
||||
if (!nn_control::outputs_finite(_output_tensor->data.f, nn_control::kOutputSize)) {
|
||||
// One bad channel would stop one motor while the other three keep thrusting.
|
||||
// Repeat the last command instead and let the commander leave the mode. The
|
||||
// network is not trusted again until it has.
|
||||
_output_fault = true;
|
||||
/* EVENT
|
||||
* @description
|
||||
* The last command is repeated and the mode reports itself unable to run so the commander
|
||||
* leaves it. The network is not run again until the mode has been left.
|
||||
*/
|
||||
events::send(events::ID("mc_nn_control_output_not_finite"), events::Log::Error,
|
||||
"Neural control: network output not finite, not running the network");
|
||||
HoldLastCommand();
|
||||
perf_end(_loop_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert the output tensor to actuator values
|
||||
RescaleActions();
|
||||
|
||||
for (int i = 0; i < nn_control::kOutputSize; i++) {
|
||||
_last_command[i] = _output_tensor->data.f[i];
|
||||
}
|
||||
|
||||
_have_last_command = true;
|
||||
PublishOutput(_output_tensor->data.f);
|
||||
|
||||
int32_t full_controller_time = GetTime() - start_time1;
|
||||
|
||||
@@ -46,7 +46,7 @@
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include <tflite_micro/tensorflow/lite/micro/micro_mutable_op_resolver.h>
|
||||
@@ -56,6 +56,8 @@
|
||||
// Include model
|
||||
#include "control_net.hpp"
|
||||
#include "actions_rescale.hpp"
|
||||
#include "nn_control_checks.hpp"
|
||||
#include "nn_control_model.hpp"
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
@@ -82,9 +84,11 @@
|
||||
|
||||
using namespace time_literals; // For the 1_s in the subscription interval
|
||||
class MulticopterNeuralNetworkControl : public ModuleBase, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
friend class NnControlStartupTestPeer;
|
||||
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
@@ -114,6 +118,8 @@ private:
|
||||
void RescaleActions();
|
||||
int InitializeNetwork();
|
||||
void UpdateMotorLimits();
|
||||
void CheckObservations();
|
||||
void HoldLastCommand();
|
||||
void ReportInvalidLimits();
|
||||
void ReportActionRange();
|
||||
int32_t GetTime();
|
||||
@@ -155,6 +161,22 @@ private:
|
||||
// set has been copied and gates the controller.
|
||||
bool _motor_limits_valid{false};
|
||||
bool _mapping_valid{false};
|
||||
|
||||
// Why the network is not being run on the current observations, checked every
|
||||
// cycle so the arming check reply always describes the current state
|
||||
nn_control::ObservationFault _observation_fault{nn_control::ObservationFault::PositionInvalid};
|
||||
// The first fault of an outage, reported once. Everything goes stale together when
|
||||
// a sensor stops, so the later faults of the same outage are not reported.
|
||||
nn_control::ObservationFault _reported_observation_fault{nn_control::ObservationFault::None};
|
||||
|
||||
// A network that produced a non finite output is not trusted again until the
|
||||
// mode is left
|
||||
bool _output_fault{false};
|
||||
|
||||
// The last command sent in this session of the mode, repeated while the
|
||||
// commander leaves the mode after a fault. Cleared whenever the mode is not active.
|
||||
float _last_command[4] {};
|
||||
bool _have_last_command{false};
|
||||
hrt_abstime _last_invalid_limits_report{0};
|
||||
float _mapping_thrust_coeff{0.f};
|
||||
float _mapping_min_rpm{0.f};
|
||||
@@ -168,10 +190,10 @@ private:
|
||||
TfLiteTensor *_input_tensor{nullptr};
|
||||
TfLiteTensor *_output_tensor{nullptr};
|
||||
float _input_data[15];
|
||||
trajectory_setpoint_s _trajectory_setpoint;
|
||||
vehicle_angular_velocity_s _angular_velocity;
|
||||
vehicle_local_position_s _position;
|
||||
vehicle_attitude_s _attitude;
|
||||
trajectory_setpoint_s _trajectory_setpoint{};
|
||||
vehicle_angular_velocity_s _angular_velocity{};
|
||||
vehicle_local_position_s _position{};
|
||||
vehicle_attitude_s _attitude{};
|
||||
manual_control_setpoint_s _manual_control_setpoint{};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
|
||||
@@ -0,0 +1,184 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file nn_control_checks.hpp
|
||||
* Checks the neural controller runs before it trusts its model, its observations
|
||||
* and its output. Header only and free of module dependencies so they can be unit
|
||||
* tested directly.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
namespace nn_control
|
||||
{
|
||||
|
||||
static constexpr int kInputSize = 15;
|
||||
static constexpr int kOutputSize = 4;
|
||||
|
||||
// Oldest observation the network is run on, in microseconds, the same limits the
|
||||
// raptor module uses
|
||||
static constexpr uint64_t kMaxLocalPositionAge = 100000;
|
||||
static constexpr uint64_t kMaxAttitudeAge = 50000;
|
||||
static constexpr uint64_t kMaxAngularVelocityAge = 10000;
|
||||
|
||||
// The controller runs on angular velocity updates. If they stop, a delayed run
|
||||
// this long after the last one is what notices.
|
||||
static constexpr uint64_t kAngularVelocityWatchdog = 2 * kMaxAngularVelocityAge;
|
||||
|
||||
enum class ObservationFault : uint8_t {
|
||||
None = 0,
|
||||
PositionInvalid = 1,
|
||||
PositionStale = 2,
|
||||
PositionNotFinite = 3,
|
||||
AttitudeStale = 4,
|
||||
AttitudeNotFinite = 5,
|
||||
AngularVelocityStale = 6,
|
||||
AngularVelocityNotFinite = 7,
|
||||
};
|
||||
|
||||
// What the network is fed, copied out of the uORB topics so the check needs
|
||||
// nothing from the module
|
||||
struct ObservationSnapshot {
|
||||
uint64_t position_timestamp;
|
||||
bool xy_valid;
|
||||
bool z_valid;
|
||||
bool v_xy_valid;
|
||||
bool v_z_valid;
|
||||
float position[3];
|
||||
float velocity[3];
|
||||
uint64_t attitude_timestamp;
|
||||
float q[4];
|
||||
uint64_t angular_velocity_timestamp;
|
||||
float angular_velocity[3];
|
||||
};
|
||||
|
||||
static inline bool all_finite(const float *values, int count)
|
||||
{
|
||||
for (int i = 0; i < count; i++) {
|
||||
if (!isfinite(values[i])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// A stamp of zero has never been set. A stamp ahead of now is not stale.
|
||||
static inline bool fresh(uint64_t now, uint64_t stamp, uint64_t max_age)
|
||||
{
|
||||
return (stamp != 0) && ((now <= stamp) || ((now - stamp) <= max_age));
|
||||
}
|
||||
|
||||
static inline ObservationFault check_observations(const ObservationSnapshot &o, uint64_t now)
|
||||
{
|
||||
if (!o.xy_valid || !o.z_valid || !o.v_xy_valid || !o.v_z_valid) {
|
||||
return ObservationFault::PositionInvalid;
|
||||
}
|
||||
|
||||
if (!fresh(now, o.position_timestamp, kMaxLocalPositionAge)) {
|
||||
return ObservationFault::PositionStale;
|
||||
}
|
||||
|
||||
if (!all_finite(o.position, 3) || !all_finite(o.velocity, 3)) {
|
||||
return ObservationFault::PositionNotFinite;
|
||||
}
|
||||
|
||||
if (!fresh(now, o.attitude_timestamp, kMaxAttitudeAge)) {
|
||||
return ObservationFault::AttitudeStale;
|
||||
}
|
||||
|
||||
if (!all_finite(o.q, 4)) {
|
||||
return ObservationFault::AttitudeNotFinite;
|
||||
}
|
||||
|
||||
if (!fresh(now, o.angular_velocity_timestamp, kMaxAngularVelocityAge)) {
|
||||
return ObservationFault::AngularVelocityStale;
|
||||
}
|
||||
|
||||
if (!all_finite(o.angular_velocity, 3)) {
|
||||
return ObservationFault::AngularVelocityNotFinite;
|
||||
}
|
||||
|
||||
return ObservationFault::None;
|
||||
}
|
||||
|
||||
static inline bool outputs_finite(const float *outputs, int count)
|
||||
{
|
||||
return all_finite(outputs, count);
|
||||
}
|
||||
|
||||
/**
|
||||
* Why a loaded model cannot be run, or nullptr when it can. The documentation
|
||||
* tells users to swap the model array, so the shape is checked rather than
|
||||
* assumed.
|
||||
*/
|
||||
static inline const char *model_layout_problem(int32_t schema_version, int32_t expected_schema_version,
|
||||
int input_tensors, int input_count, bool input_is_float,
|
||||
int output_tensors, int output_count, bool output_is_float)
|
||||
{
|
||||
if (schema_version != expected_schema_version) {
|
||||
return "schema version";
|
||||
}
|
||||
|
||||
if (input_tensors != 1) {
|
||||
return "input tensors, expected one";
|
||||
}
|
||||
|
||||
if (output_tensors != 1) {
|
||||
return "output tensors, expected one";
|
||||
}
|
||||
|
||||
if (input_count != kInputSize) {
|
||||
return "input size, expected 15 elements";
|
||||
}
|
||||
|
||||
if (!input_is_float) {
|
||||
return "input type, expected float32";
|
||||
}
|
||||
|
||||
if (output_count != kOutputSize) {
|
||||
return "output size, expected 4 elements";
|
||||
}
|
||||
|
||||
if (!output_is_float) {
|
||||
return "output type, expected float32";
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace nn_control
|
||||
@@ -0,0 +1,58 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file nn_control_model.hpp
|
||||
* The operations the shipped network needs, shared by the module and the test
|
||||
* that loads the shipped model through the real interpreter.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <tflite_micro/tensorflow/lite/micro/micro_mutable_op_resolver.h>
|
||||
|
||||
namespace nn_control
|
||||
{
|
||||
|
||||
// The number of operations in the model, fully connected, relu and add
|
||||
using OpResolver = tflite::MicroMutableOpResolver<3>;
|
||||
|
||||
static inline TfLiteStatus register_ops(OpResolver &op_resolver)
|
||||
{
|
||||
TF_LITE_ENSURE_STATUS(op_resolver.AddFullyConnected());
|
||||
TF_LITE_ENSURE_STATUS(op_resolver.AddRelu());
|
||||
TF_LITE_ENSURE_STATUS(op_resolver.AddAdd());
|
||||
return kTfLiteOk;
|
||||
}
|
||||
|
||||
} // namespace nn_control
|
||||
Reference in new issue
Block a user