diff --git a/Makefile b/Makefile index b5d950f53ab..97bd60612da 100644 --- a/Makefile +++ b/Makefile @@ -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) diff --git a/docs/en/neural_networks/tflm.md b/docs/en/neural_networks/tflm.md index 57b2847867d..f38ecdda625 100644 --- a/docs/en/neural_networks/tflm.md +++ b/docs/en/neural_networks/tflm.md @@ -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 diff --git a/src/modules/mc_nn_control/CMakeLists.txt b/src/modules/mc_nn_control/CMakeLists.txt index 2675df4988a..7ace27229ca 100644 --- a/src/modules/mc_nn_control/CMakeLists.txt +++ b/src/modules/mc_nn_control/CMakeLists.txt @@ -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) diff --git a/src/modules/mc_nn_control/NnControlChecksTest.cpp b/src/modules/mc_nn_control/NnControlChecksTest.cpp new file mode 100644 index 00000000000..68fdc49f20e --- /dev/null +++ b/src/modules/mc_nn_control/NnControlChecksTest.cpp @@ -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 +#include +#include + +#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"); +} diff --git a/src/modules/mc_nn_control/NnControlModelTest.cpp b/src/modules/mc_nn_control/NnControlModelTest.cpp new file mode 100644 index 00000000000..6160672a636 --- /dev/null +++ b/src/modules/mc_nn_control/NnControlModelTest.cpp @@ -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 + +#include +#include + +#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; +} diff --git a/src/modules/mc_nn_control/NnControlStartupTest.cpp b/src/modules/mc_nn_control/NnControlStartupTest.cpp new file mode 100644 index 00000000000..0466968d196 --- /dev/null +++ b/src/modules/mc_nn_control/NnControlStartupTest.cpp @@ -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 +#include +#include + +#include "mc_nn_control.hpp" + +#include +#include +#include +#include + +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(); +} diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index c33c96f296c..15c0bae4ed8 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -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(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; diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index e14eca1c685..62f241316e2 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include @@ -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 #include @@ -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( diff --git a/src/modules/mc_nn_control/nn_control_checks.hpp b/src/modules/mc_nn_control/nn_control_checks.hpp new file mode 100644 index 00000000000..ffac3d1f75c --- /dev/null +++ b/src/modules/mc_nn_control/nn_control_checks.hpp @@ -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 +#include + +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 diff --git a/src/modules/mc_nn_control/nn_control_model.hpp b/src/modules/mc_nn_control/nn_control_model.hpp new file mode 100644 index 00000000000..39f73afb2d4 --- /dev/null +++ b/src/modules/mc_nn_control/nn_control_model.hpp @@ -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 + +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