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:
Saibernard authored and GitHub committed 2026-10-03 11:27:21 -06:00
1 parent b12d17f632
commit 4da3cff386
10 files changed
+854 -41

No files matched your search

+1 -1
View File
@@ -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)
+3
View File
@@ -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
+10
View File
@@ -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();
}
+186 -34
View File
@@ -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;
+28 -6
View File
@@ -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