From b492664e4187ffc697d2a85cf811c5fe7448fccf Mon Sep 17 00:00:00 2001 From: Saibernard <112599512+Saibernard@users.noreply.github.com> Date: Wed, 30 Sep 2026 17:26:44 -0400 Subject: [PATCH] fix(mc_nn_control): correct errors and add the first unit tests (#28433) * fix(mc_nn_control): use single precision sqrt and correct two parameter/comment errors Three small corrections in the neural network controller, none of which change the commanded thrust for a valid configuration: - RescaleActions() called sqrt() rather than sqrtf(), promoting to double in a loop that runs once per angular velocity sample. On an FPU without double precision that is a software routine, four times per cycle, in the path whose execution time the module exists to measure. - MC_NN_THRST_COEF declared a minimum of 0.0, but the value is used as a divisor in RescaleActions(). Zero is therefore an in-range setting that makes the motor scaling non-finite. Raise the minimum off zero. - The comment above PopulateInputTensor() gives the observation order as [pos_err(3), lin_vel(3), att(6), ang_vel(3)], but the code below it, NeuralControl.msg and the module usage text all use [pos_err(3), att(6), lin_vel(3), ang_vel(3)]. Attitude and linear velocity are transposed in the comment only. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * refactor(mc_nn_control): extract the action rescaling into a testable header The mapping from network action to motor command lived inside a private member operating on the TFLM output tensor, so it could not be unit tested. Move the per element math into a dependency free header function. Behaviour is unchanged. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * fix(mc_nn_control): keep motor commands between idle and full scale The action to motor command mapping ran past both ends of the motor. On the shipped defaults action -1 mapped to -0.0077, which the motor output treats as a stopped motor while the other three keep thrusting, and every action above 0.61 mapped past full scale. With a minimum rpm above a ninth of the maximum the last stage also ran backwards for the first few thousandths of the range. All three come from normalizing the rpm against limits the thrust coefficient does not reach. Reported in #28417. Bound the normalized rpm between the limits, so a thrust below what the minimum rpm gives idles the motor and one above the maximum runs it at full scale, and write the thrust curve compensation as a x^2 + (1 - a) x so idle is exactly 0 and full scale exactly 1. The curve inside the limits is unchanged. Validate the three parameters at start and on every parameter update: finite, the coefficient above zero, the minimum at least zero and below the maximum, and a part of the action range wider than float resolution reaching the motor, so rounding cannot pass a window with no action inside it. Raw parameter writes are not bounded by the metadata, so this is checked in code. The mapping only ever uses a set that passed. An invalid set is reported as an error event, repeated while it stands, and the arming check for the mode is refused until it is corrected, so the commander neither arms into the mode nor keeps flying it. The mapping keeps the last valid set meanwhile, so the write itself never steps the motors. A valid set that covers less than the whole action range is reported as a warning naming what it does cover, when it changes and again when the mode is entered. The arming check reply was not zero initialized, which left two mode requirement fields to whatever was on the stack. Also from #28417: the loop performance counter was left open on the two early returns after inference, and actuator_motors.timestamp_sample was never set. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * test(mc_nn_control): add the module's first unit tests The action to motor command mapping had no tests. These assert the properties it has to keep rather than any particular output value: actions beyond the boundary clamp, every command stays between 0 and 1, the lowest action idles the motor and the highest runs it at full scale, the mapping never decreases and rises inside the achievable range, actions outside that range sit on the bounds, the output stays finite for every normal action and across the documented parameter ranges, a non finite action gives no command, and invalid limits are rejected. Every threshold comes from the mapping's own achievable range, and each property is checked for the shipped defaults, a smaller motor, and a motor whose limits cover the whole action range, so a change to the defaults only fails them when a property is broken. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * ci(tests): run the mc_nn_control unit tests in the neural configuration make tests builds px4_sitl_test, which does not enable MC_NN_CONTROL, so the unit tests the module registers were never built or run upstream. The only configuration that enables the module is px4_sitl_neural. Add a tests_neural target that builds that configuration with testing on and runs the module's tests, the same shape as tests_daa_crosstrack and tests_vtest_moving, and run it from the Unit Tests job. Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Saibernard Yogendran --------- Signed-off-by: Saibernard Yogendran --- .github/workflows/checks.yml | 1 + Makefile | 11 +- src/modules/mc_nn_control/CMakeLists.txt | 3 + .../mc_nn_control/RescaleActionTest.cpp | 273 ++++++++++++++++++ src/modules/mc_nn_control/actions_rescale.hpp | 150 ++++++++++ src/modules/mc_nn_control/mc_nn_control.cpp | 118 ++++++-- src/modules/mc_nn_control/mc_nn_control.hpp | 15 + .../mc_nn_control/mc_nn_control_params.yaml | 8 +- 8 files changed, 548 insertions(+), 31 deletions(-) create mode 100644 src/modules/mc_nn_control/RescaleActionTest.cpp create mode 100644 src/modules/mc_nn_control/actions_rescale.hpp diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index 1e3009e672d..df9f84b0645 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -79,6 +79,7 @@ jobs: make tests make tests_daa_crosstrack make tests_vtest_moving + make tests_neural - uses: ./.github/actions/save-ccache if: always() diff --git a/Makefile b/Makefile index 43dc242342d..b5d950f53ab 100644 --- a/Makefile +++ b/Makefile @@ -445,7 +445,7 @@ check_newlines: # Testing # -------------------------------------------------------------------- -.PHONY: tests tests_daa_crosstrack tests_vtest_moving tests_coverage tests_mission tests_mission_coverage tests_offboard +.PHONY: tests tests_daa_crosstrack tests_vtest_moving tests_neural tests_coverage tests_mission tests_mission_coverage tests_offboard .PHONY: rostest python_coverage tests: @@ -473,6 +473,15 @@ tests_vtest_moving: $(eval UBSAN_OPTIONS += color=always) $(call cmake-build,px4_sitl_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 += -DCMAKE_TESTING=ON) + $(eval ARGS += test_results) + $(eval ASAN_OPTIONS += color=always:check_initialization_order=1:detect_stack_use_after_return=1) + $(eval UBSAN_OPTIONS += color=always) + $(call cmake-build,px4_sitl_neural) + # work around lcov bug #316; remove once lcov is fixed (see https://github.com/linux-test-project/lcov/issues/316) LCOBUG = --ignore-errors mismatch,negative tests_coverage: diff --git a/src/modules/mc_nn_control/CMakeLists.txt b/src/modules/mc_nn_control/CMakeLists.txt index ce28324bcf6..2675df4988a 100644 --- a/src/modules/mc_nn_control/CMakeLists.txt +++ b/src/modules/mc_nn_control/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_module( SRCS mc_nn_control.cpp mc_nn_control.hpp + actions_rescale.hpp control_net.cpp control_net.hpp MODULE_CONFIG @@ -54,3 +55,5 @@ px4_add_module( target_link_libraries(mc_nn_control PRIVATE tensorflow_lite_micro) target_include_directories(mc_nn_control PRIVATE ${CMAKE_SOURCE_DIR}/src/lib/tensorflow_lite_micro) + +px4_add_unit_gtest(SRC RescaleActionTest.cpp) diff --git a/src/modules/mc_nn_control/RescaleActionTest.cpp b/src/modules/mc_nn_control/RescaleActionTest.cpp new file mode 100644 index 00000000000..1fa1f1d0066 --- /dev/null +++ b/src/modules/mc_nn_control/RescaleActionTest.cpp @@ -0,0 +1,273 @@ +/**************************************************************************** +* +* 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 RescaleActionTest.cpp + * Unit tests for the network action to motor command mapping. They assert the + * properties the mapping has to keep rather than any particular output value, + * and derive every threshold from the mapping's own achievable range, so a + * change to the defaults only fails them when a property is broken. + * + * to run, on a config that enables the module: + * cmake -DCMAKE_TESTING=ON build/px4_sitl_neural + * ninja -C build/px4_sitl_neural unit-RescaleAction + * ctest --test-dir build/px4_sitl_neural -R RescaleAction + */ + +#include +#include + +#include "actions_rescale.hpp" + +using nn_control::rescale_action; +using nn_control::achievable_action_range; +using nn_control::valid_motor_limits; + +struct Params { + float thrust_coeff; + float min_rpm; + float max_rpm; +}; + +// The shipped defaults from mc_nn_control_params.yaml, a smaller motor whose +// minimum rpm is above a ninth of its maximum, and a motor whose limits cover +// the whole action range. The properties below hold for all of them. +static constexpr Params kDefaults{1.2f, 1000.f, 22000.f}; +static constexpr Params kSmallMotor{3.0f, 2000.f, 12000.f}; +static constexpr Params kMatchedMotor{1.2f, 0.f, 24495.f}; +static constexpr Params kParamSets[] = {kDefaults, kSmallMotor, kMatchedMotor}; + +static float map(float action, const Params &p) +{ + return rescale_action(action, p.thrust_coeff, p.min_rpm, p.max_rpm); +} + +static void range(const Params &p, float &lowest, float &highest) +{ + achievable_action_range(p.thrust_coeff, p.min_rpm, p.max_rpm, lowest, highest); +} + +TEST(RescaleActionTest, actionsBeyondOneClampToTheBoundary) +{ + for (const Params &p : kParamSets) { + EXPECT_FLOAT_EQ(map(-5.f, p), map(-1.f, p)); + EXPECT_FLOAT_EQ(map(5.f, p), map(1.f, p)); + } +} + +TEST(RescaleActionTest, commandsStayWithinZeroAndOne) +{ + for (const Params &p : kParamSets) { + for (float action = -1.f; action <= 1.f; action += 0.01f) { + const float cmd = map(action, p); + EXPECT_GE(cmd, 0.f) << "below zero at action " << action; + EXPECT_LE(cmd, 1.f) << "above one at action " << action; + } + } +} + +TEST(RescaleActionTest, lowestActionIdlesTheMotorAndHighestRunsItAtFullScale) +{ + for (const Params &p : kParamSets) { + EXPECT_FLOAT_EQ(map(-1.f, p), 0.f); + // a motor whose maximum rpm is only just reached at action one lands a + // rounding step short of it + EXPECT_NEAR(map(1.f, p), 1.f, 1e-4f); + } +} + +TEST(RescaleActionTest, mappingDoesNotDecrease) +{ + for (const Params &p : kParamSets) { + float prev = map(-1.f, p); + + for (float action = -0.99f; action <= 1.f; action += 0.01f) { + const float cmd = map(action, p); + EXPECT_GE(cmd, prev) << "decreases at action " << action; + prev = cmd; + } + } +} + +TEST(RescaleActionTest, mappingRisesInsideTheAchievableRange) +{ + for (const Params &p : kParamSets) { + float lowest, highest; + range(p, lowest, highest); + const float start = fmaxf(lowest, -1.f); + const float end = fminf(highest, 1.f); + float prev = map(start, p); + + for (float action = start + 0.01f; action < end; action += 0.01f) { + const float cmd = map(action, p); + EXPECT_GT(cmd, prev) << "flat at action " << action; + prev = cmd; + } + } +} + +TEST(RescaleActionTest, actionsOutsideTheAchievableRangeSitOnTheBounds) +{ + for (const Params &p : kParamSets) { + float lowest, highest; + range(p, lowest, highest); + + if (lowest > -1.f) { + EXPECT_FLOAT_EQ(map(lowest - 0.001f, p), 0.f); + } + + if (highest < 1.f) { + EXPECT_FLOAT_EQ(map(highest + 0.001f, p), 1.f); + EXPECT_LT(map(highest - 0.01f, p), 1.f); + } + } +} + +TEST(RescaleActionTest, achievableRangeFollowsTheLimits) +{ + float lowest, highest; + range(kMatchedMotor, lowest, highest); + EXPECT_NEAR(lowest, -1.f, 1e-3f); + EXPECT_NEAR(highest, 1.f, 1e-3f); + + range(kDefaults, lowest, highest); + EXPECT_GT(lowest, -1.f); + EXPECT_LT(highest, 1.f); +} + +TEST(RescaleActionTest, outputsRemainFiniteForNormalInputs) +{ + for (const Params &p : kParamSets) { + for (float action = -1.f; action <= 1.f; action += 0.01f) { + EXPECT_TRUE(std::isfinite(map(action, p))) << "not finite at action " << action; + } + } +} + +// Every accepted set of limits has to give a real command somewhere inside the +// action range, and every rejected set has to give none. The intermediate command +// check does not go through the validator, so it is an independent oracle for it. +static void check_limits(float thrust_coeff, float min_rpm, float max_rpm, int &valid_sets, int &rejected_sets) +{ + const bool valid = valid_motor_limits(thrust_coeff, min_rpm, max_rpm); + valid ? valid_sets++ : rejected_sets++; + + for (float action = -1.f; action <= 1.f; action += 0.25f) { + EXPECT_EQ(std::isfinite(rescale_action(action, thrust_coeff, min_rpm, max_rpm)), valid) + << "thrust_coeff " << thrust_coeff << " min " << min_rpm << " max " << max_rpm << " action " << action; + } + + if (valid) { + float lowest, highest; + achievable_action_range(thrust_coeff, min_rpm, max_rpm, lowest, highest); + const float middle = (fmaxf(lowest, -1.f) + fminf(highest, 1.f)) / 2.f; + const float cmd = rescale_action(middle, thrust_coeff, min_rpm, max_rpm); + EXPECT_GT(cmd, 0.f) << "thrust_coeff " << thrust_coeff << " min " << min_rpm << " max " << max_rpm; + EXPECT_LT(cmd, 1.f) << "thrust_coeff " << thrust_coeff << " min " << min_rpm << " max " << max_rpm; + } +} + +TEST(RescaleActionTest, acceptedLimitsCommandSomethingBetweenIdleAndFullScale) +{ + int valid_sets = 0; + int rejected_sets = 0; + + // the limits from mc_nn_control_params.yaml, with the rpm pair kept apart + for (float thrust_coeff = 0.01f; thrust_coeff <= 5.f; thrust_coeff += 0.05f) { + for (float min_rpm = 0.f; min_rpm < 80000.f; min_rpm += 8000.f) { + for (float max_rpm = min_rpm + 500.f; max_rpm <= 80000.f; max_rpm += 8000.f) { + check_limits(thrust_coeff, min_rpm, max_rpm, valid_sets, rejected_sets); + } + } + } + + // narrow windows near the bottom of the range, where float rounding once let a + // set through whose only outputs were the two clamps, up to bands wide enough + // to be accepted again + for (float max_rpm = 1.f; max_rpm <= 4000.f; max_rpm += 1.f) { + check_limits(0.01f, 0.f, max_rpm, valid_sets, rejected_sets); + } + + EXPECT_GT(valid_sets, 0); + EXPECT_GT(rejected_sets, 0); +} + +TEST(RescaleActionTest, nonFiniteActionGivesNoCommand) +{ + // the caller maps a non finite command to a stopped motor, so the mapping + // must not turn a bad action into a number + for (const Params &p : kParamSets) { + EXPECT_FALSE(std::isfinite(map(NAN, p))); + } +} + +TEST(RescaleActionTest, invalidLimitsAreRejected) +{ + // ordering and sign + EXPECT_FALSE(valid_motor_limits(1.2f, 5000.f, 5000.f)); + EXPECT_FALSE(valid_motor_limits(1.2f, 6000.f, 5000.f)); + EXPECT_FALSE(valid_motor_limits(0.f, 1000.f, 22000.f)); + EXPECT_FALSE(valid_motor_limits(-1.2f, 1000.f, 22000.f)); + EXPECT_FALSE(valid_motor_limits(1.2f, -1.f, 22000.f)); + + // raw parameter writes are not bounded by the metadata, so non finite values + // have to be caught here. An infinite coefficient would map every action to + // idle while looking valid. + EXPECT_FALSE(valid_motor_limits(INFINITY, 1000.f, 22000.f)); + EXPECT_FALSE(valid_motor_limits(NAN, 1000.f, 22000.f)); + EXPECT_FALSE(valid_motor_limits(1.2f, NAN, 22000.f)); + EXPECT_FALSE(valid_motor_limits(1.2f, 1000.f, INFINITY)); + + // limits the action range cannot reach: idle already above action one, or + // full scale still below action minus one, or a window too narrow to hold a + // representable action + EXPECT_FALSE(valid_motor_limits(5.f, 79999.f, 80000.f)); + EXPECT_FALSE(valid_motor_limits(0.01f, 0.f, 1.f)); + EXPECT_FALSE(valid_motor_limits(0.01f, 0.f, 33.f)); + + // values that overflow the achievable range would otherwise clip to a full span + EXPECT_FALSE(valid_motor_limits(1e38f, 0.f, 1e38f)); + + // a narrow band is still a band. This motor answers to five percent of the + // action range and has to be accepted, the range warning is what tells the + // user about it. + EXPECT_TRUE(valid_motor_limits(1.2f, 1000.f, 3999.f)); + + for (const Params &p : kParamSets) { + EXPECT_TRUE(valid_motor_limits(p.thrust_coeff, p.min_rpm, p.max_rpm)); + } + + EXPECT_FALSE(std::isfinite(rescale_action(0.f, 1.2f, 5000.f, 5000.f))); + EXPECT_FALSE(std::isfinite(rescale_action(0.f, INFINITY, 1000.f, 22000.f))); + EXPECT_FALSE(std::isfinite(rescale_action(0.f, 5.f, 79999.f, 80000.f))); +} diff --git a/src/modules/mc_nn_control/actions_rescale.hpp b/src/modules/mc_nn_control/actions_rescale.hpp new file mode 100644 index 00000000000..c000aa9ef42 --- /dev/null +++ b/src/modules/mc_nn_control/actions_rescale.hpp @@ -0,0 +1,150 @@ +/**************************************************************************** +* +* 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 actions_rescale.hpp + * Maps a network action to a normalized motor command. Header only and free of + * module dependencies so it can be unit tested directly. + * + * The network acts in [-1, 1]. An action is read as a per motor thrust of + * action + 1 in the units the thrust coefficient was fitted in, turned into an + * rpm through that coefficient, and placed between the motor's rpm limits. The + * last step compensates the motor's thrust curve. + */ + +#pragma once + +#include + +namespace nn_control +{ + +static constexpr float kThrustCoeffScale = 100000.0f; + +// The part of the action range that reaches the motor has to be at least this +// wide, about a thousand representable actions at the edge of the range. Narrower +// than this and float rounding can leave a window with no action inside it, idle +// on one side and full scale on the other. It says nothing about how narrow a +// band a user may want, the range warning reports that. +static constexpr float kMinAchievableActionSpan = 1e-4f; + +/** + * The part of the action range the configured motor can reproduce. Actions below + * lowest idle the motor, actions above highest run it at full scale. + */ +static inline void achievable_action_range(float thrust_coeff_raw, float min_rpm, float max_rpm, + float &lowest, float &highest) +{ + const float thrust_coeff = thrust_coeff_raw / kThrustCoeffScale; + lowest = thrust_coeff * (min_rpm / 60.f) * (min_rpm / 60.f) - 1.f; + highest = thrust_coeff * (max_rpm / 60.f) * (max_rpm / 60.f) - 1.f; +} + +/** + * True when the limits describe a motor the mapping can work with: finite, the + * coefficient above zero, 0 <= min < max, and a part of the action range at least + * kMinAchievableActionSpan wide reaching something between idle and full scale. + * The parameter metadata bounds each value on its own, this checks them together + * and against raw writes. + */ +static inline bool valid_motor_limits(float thrust_coeff_raw, float min_rpm, float max_rpm) +{ + if (!isfinite(thrust_coeff_raw) || !isfinite(min_rpm) || !isfinite(max_rpm)) { + return false; + } + + if (!(thrust_coeff_raw > 0.f) || !(min_rpm >= 0.f) || !(max_rpm > min_rpm)) { + return false; + } + + float lowest = 0.f; + float highest = 0.f; + achievable_action_range(thrust_coeff_raw, min_rpm, max_rpm, lowest, highest); + + // values large enough to overflow the range would clip to a full span + if (!isfinite(lowest) || !isfinite(highest)) { + return false; + } + + const float usable_low = (lowest > -1.f) ? lowest : -1.f; + const float usable_high = (highest < 1.f) ? highest : 1.f; + return (usable_high - usable_low) > kMinAchievableActionSpan; +} + +/** + * Map one network action in [-1, 1] to a normalized motor command in [0, 1]. + * + * @param action network output, clamped to [-1, 1] + * @param thrust_coeff_raw MC_NN_THRST_COEF as stored (scaled by 1e-5 internally) + * @param min_rpm MC_NN_MIN_RPM + * @param max_rpm MC_NN_MAX_RPM + * @return the command, NAN for a non finite action or limits that fail valid_motor_limits() + */ +static inline float rescale_action(float action, float thrust_coeff_raw, float min_rpm, float max_rpm) +{ + if (!valid_motor_limits(thrust_coeff_raw, min_rpm, max_rpm) || !isfinite(action)) { + return NAN; + } + + const float thrust_coeff = thrust_coeff_raw / kThrustCoeffScale; + + if (action < -1.0f) { + action = -1.0f; + + } else if (action > 1.0f) { + action = 1.0f; + } + + const float rpm = 60.0f * sqrtf((action + 1.0f) / thrust_coeff); + + // Where that rpm sits between the motor's limits. Below the minimum the motor + // idles, above the maximum it is at full scale. Without the two bounds the + // bottom of the action range went negative, which the motor output treats as + // disarmed, and the top went past full scale. + float x = (rpm - min_rpm) / (max_rpm - min_rpm); + + if (x < 0.f) { + x = 0.f; + + } else if (x > 1.f) { + x = 1.f; + } + + // Thrust curve compensation, a * x^2 + (1 - a) * x, which is 0 at idle and 1 + // at full scale exactly. This is the same polynomial the module always used, + // written out instead of in vertex form. + const float a = 0.8f; + return a * x * x + (1.0f - a) * x; +} + +} // namespace nn_control diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 1b2bba08e8a..c33c96f296c 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -39,6 +39,9 @@ */ #include "mc_nn_control.hpp" +#include + +using namespace time_literals; #ifdef __PX4_NUTTX #include #else @@ -87,9 +90,79 @@ bool MulticopterNeuralNetworkControl::init() return false; } + updateParams(); + UpdateMotorLimits(); + return true; } +void MulticopterNeuralNetworkControl::UpdateMotorLimits() +{ + const float thrust_coeff = _param_thrust_coeff.get(); + const float min_rpm = _param_min_rpm.get(); + const float max_rpm = _param_max_rpm.get(); + + if (!nn_control::valid_motor_limits(thrust_coeff, min_rpm, max_rpm)) { + // The mapping keeps whatever was valid before, so a write while flying does + // not step the motors. The arming check reply refuses the mode until the + // parameters are corrected, and the error repeats while they stand. + if (_motor_limits_valid || (_last_invalid_limits_report == 0)) { + ReportInvalidLimits(); + } + + _motor_limits_valid = false; + return; + } + + // Any parameter on the system triggers an update, so only report when the + // limits themselves changed + const bool unchanged = _mapping_valid && (thrust_coeff == _mapping_thrust_coeff) && (min_rpm == _mapping_min_rpm) + && (max_rpm == _mapping_max_rpm); + _motor_limits_valid = true; + + if (unchanged) { + return; + } + + _mapping_thrust_coeff = thrust_coeff; + _mapping_min_rpm = min_rpm; + _mapping_max_rpm = max_rpm; + _mapping_valid = true; + + ReportActionRange(); +} + +void MulticopterNeuralNetworkControl::ReportInvalidLimits() +{ + _last_invalid_limits_report = hrt_absolute_time(); + /* EVENT + * @description + * The limits have to be finite, with MC_NN_THRST_COEF above zero, MC_NN_MIN_RPM at least zero and + * below MC_NN_MAX_RPM, and reaching at least part of the network's action range. The mapping keeps + * the last valid set and the mode refuses the arming check until they are corrected. + */ + events::send(events::ID("mc_nn_control_motor_limits_invalid"), events::Log::Error, + "Neural control: invalid motor limits, MC_NN_MIN_RPM {2}, MC_NN_MAX_RPM {1}, MC_NN_THRST_COEF {3:.2}", + (int32_t)_param_max_rpm.get(), (int32_t)_param_min_rpm.get(), _param_thrust_coeff.get()); +} + +void MulticopterNeuralNetworkControl::ReportActionRange() +{ + // Say which part of the action range this motor can reproduce, so a mismatch + // between the thrust coefficient and the rpm limits is visible rather than + // silently clipped in flight. Sent when the limits change and when the mode + // is entered, since a message from boot can pass before a link is up. + float lowest = 0.f; + float highest = 0.f; + nn_control::achievable_action_range(_mapping_thrust_coeff, _mapping_min_rpm, _mapping_max_rpm, lowest, highest); + + if ((lowest > -0.95f) || (highest < 0.95f)) { + events::send(events::ID("mc_nn_control_action_range"), events::Log::Warning, + "Neural control: motor limits cover actions {1:.2} to {2:.2} of -1 to 1", + math::max(lowest, -1.f), math::min(highest, 1.f)); + } +} + int MulticopterNeuralNetworkControl::InitializeNetwork() { if (_interpreter != nullptr) { @@ -193,13 +266,20 @@ void MulticopterNeuralNetworkControl::ConfigureNeuralFlightMode(int8 mode_id) void MulticopterNeuralNetworkControl::ReplyToArmingCheck(int8 request_id) { // Reply to the arming check request - arming_check_reply_s arming_check_reply; + arming_check_reply_s arming_check_reply{}; arming_check_reply.timestamp = hrt_absolute_time(); arming_check_reply.request_id = 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 = true; + arming_check_reply.can_arm_and_run = _motor_limits_valid; + + // The reason is a standalone event, repeated while the limits stay invalid so + // it is not lost on a link that came up later + if (!_motor_limits_valid && (hrt_elapsed_time(&_last_invalid_limits_report) > 10_s)) { + ReportInvalidLimits(); + } + arming_check_reply.mode_req_angular_velocity = true; arming_check_reply.mode_req_local_position = true; arming_check_reply.mode_req_attitude = true; @@ -296,7 +376,7 @@ void MulticopterNeuralNetworkControl::generate_trajectory_setpoint(float dt) void MulticopterNeuralNetworkControl::PopulateInputTensor() { - // Creates a 15 element input tensor for the neural network [pos_err(3), lin_vel(3), att(6), ang_vel(3)] + // Creates a 15 element input tensor for the neural network [pos_err(3), att(6), lin_vel(3), ang_vel(3)] // transform observations in correct frame matrix::Dcmf frame_transf; @@ -373,6 +453,7 @@ void MulticopterNeuralNetworkControl::PublishOutput(float *command_actions) actuator_motors_s actuator_motors; actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = _angular_velocity.timestamp_sample; actuator_motors.control[0] = PX4_ISFINITE(command_actions[0]) ? command_actions[0] : NAN; actuator_motors.control[1] = PX4_ISFINITE(command_actions[1]) ? command_actions[1] : NAN; @@ -395,30 +476,9 @@ void MulticopterNeuralNetworkControl::PublishOutput(float *command_actions) inline void MulticopterNeuralNetworkControl::RescaleActions() { - const float thrust_coeff = _param_thrust_coeff.get() / 100000.0f; - const float min_rpm = _param_min_rpm.get(); - const float max_rpm = _param_max_rpm.get(); - const float a = 0.8f; - const float b = (1.0f - 0.8f); - const float tmp1 = b / (2.f * a); - const float tmp2 = b * b / (4.f * a * a); - for (int i = 0; i < 4; i++) { - - if (_output_tensor->data.f[i] < -1.0f) { - _output_tensor->data.f[i] = -1.0f; - - } else if (_output_tensor->data.f[i] > 1.0f) { - _output_tensor->data.f[i] = 1.0f; - } - - _output_tensor->data.f[i] = _output_tensor->data.f[i] + 1.0f; - float rps = _output_tensor->data.f[i] / thrust_coeff; - rps = sqrt(rps); - float rpm = rps * 60.0f; - _output_tensor->data.f[i] = (rpm * 2.0f - max_rpm - min_rpm) / (max_rpm - min_rpm); - _output_tensor->data.f[i] = a * (((_output_tensor->data.f[i] + 1.0f) / 2.0f + tmp1) * (( - _output_tensor->data.f[i] + 1.0f) / 2.0f + tmp1) - tmp2); + _output_tensor->data.f[i] = nn_control::rescale_action(_output_tensor->data.f[i], + _mapping_thrust_coeff, _mapping_min_rpm, _mapping_max_rpm); } } @@ -495,6 +555,7 @@ void MulticopterNeuralNetworkControl::Run() if (!prev_use_neural && _use_neural) { ConfigureNeuralFlightMode(_mode_id); + ReportActionRange(); } } @@ -502,9 +563,10 @@ void MulticopterNeuralNetworkControl::Run() parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); updateParams(); + UpdateMotorLimits(); } - if (!_use_neural) { + if (!_use_neural || !_mapping_valid) { // If the neural network flight mode is not enabled, do nothing perf_end(_loop_perf); return; @@ -564,6 +626,7 @@ void MulticopterNeuralNetworkControl::Run() if (invoke_status != kTfLiteOk) { PX4_ERR("Invoke() failed"); + perf_end(_loop_perf); return; } @@ -571,6 +634,7 @@ void MulticopterNeuralNetworkControl::Run() if (_output_tensor == nullptr) { PX4_ERR("Output tensor is null"); + perf_end(_loop_perf); return; } diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index 4a9b6abeafd..e14eca1c685 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -55,6 +55,7 @@ // Include model #include "control_net.hpp" +#include "actions_rescale.hpp" #include #include @@ -112,6 +113,9 @@ private: void PublishOutput(float *command_actions); void RescaleActions(); int InitializeNetwork(); + void UpdateMotorLimits(); + void ReportInvalidLimits(); + void ReportActionRange(); int32_t GetTime(); void RegisterNeuralFlightMode(); void UnregisterNeuralFlightMode(int8 arming_check_id, int8 mode_id); @@ -144,6 +148,17 @@ private: // Variables bool _use_neural{false}; bool _sent_mode_registration{false}; + + // Motor limits the mapping runs with. Only a set that passed validation is + // copied here, so a bad parameter write cannot reach the motors. _motor_limits_valid + // follows the current parameters and gates the arming check, _mapping_valid says a + // set has been copied and gates the controller. + bool _motor_limits_valid{false}; + bool _mapping_valid{false}; + hrt_abstime _last_invalid_limits_report{0}; + float _mapping_thrust_coeff{0.f}; + float _mapping_min_rpm{0.f}; + float _mapping_max_rpm{0.f}; perf_counter_t _loop_perf; /**< loop duration performance counter */ hrt_abstime _last_run{0}; uint8 _mode_request_id{231}; //Random value diff --git a/src/modules/mc_nn_control/mc_nn_control_params.yaml b/src/modules/mc_nn_control/mc_nn_control_params.yaml index 3819ba2d2ef..6cb2ff72807 100644 --- a/src/modules/mc_nn_control/mc_nn_control_params.yaml +++ b/src/modules/mc_nn_control/mc_nn_control_params.yaml @@ -11,7 +11,9 @@ parameters: description: short: Max motor RPM for neural network normalization long: The maximum RPM of the motors. Used to normalize the output of the neural - network + network. Has to be above MC_NN_MIN_RPM. Together with MC_NN_THRST_COEF it + sets the part of the action range the motor can reproduce, which the module + reports at startup. type: int32 default: 22000 min: 0 @@ -20,7 +22,7 @@ parameters: description: short: Min motor RPM for neural network normalization long: The minimum RPM of the motors. Used to normalize the output of the neural - network + network. Actions that ask for less than this idle the motor. type: int32 default: 1000 min: 0 @@ -32,7 +34,7 @@ parameters: neural network. Divided by 100 000 type: float default: 1.2 - min: 0.0 + min: 0.01 max: 5.0 MC_NN_MANL_CTRL: description: