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: