mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-10-06 09:02:52 +08:00
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 <bernie97@seas.upenn.edu> * 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 <bernie97@seas.upenn.edu> * 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 <bernie97@seas.upenn.edu> * 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 <bernie97@seas.upenn.edu> * 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 <bernie97@seas.upenn.edu> --------- Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>
This commit is contained in:
@@ -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()
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 <math.h>
|
||||
|
||||
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
|
||||
@@ -39,6 +39,9 @@
|
||||
*/
|
||||
|
||||
#include "mc_nn_control.hpp"
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
using namespace time_literals;
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <drivers/drv_hrt.h>
|
||||
#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<int32_t, int32_t, float>(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<float, float>(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;
|
||||
}
|
||||
|
||||
|
||||
@@ -55,6 +55,7 @@
|
||||
|
||||
// Include model
|
||||
#include "control_net.hpp"
|
||||
#include "actions_rescale.hpp"
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user