diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 2335dc2eb0..2616e02e10 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -34,9 +34,6 @@ #include "ManualControl.hpp" #include -namespace manual_control -{ - ManualControl::ManualControl() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) @@ -103,19 +100,15 @@ void ManualControl::Run() // User override by stick const float dt_s = (now - _last_time) / 1e6f; - _x_diff.update(_selector.setpoint().chosen_input.x, dt_s); - _y_diff.update(_selector.setpoint().chosen_input.y, dt_s); - _z_diff.update(_selector.setpoint().chosen_input.z, dt_s); - _r_diff.update(_selector.setpoint().chosen_input.r, dt_s); - const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); - const bool rpy_moved = (fabsf(_x_diff.diff()) > minimum_stick_change) - || (fabsf(_y_diff.diff()) > minimum_stick_change) - || (fabsf(_r_diff.diff()) > minimum_stick_change); + const bool rpy_moved = (fabsf(_x_diff.update(_selector.setpoint().chosen_input.x, dt_s)) > minimum_stick_change) + || (fabsf(_y_diff.update(_selector.setpoint().chosen_input.y, dt_s)) > minimum_stick_change) + || (fabsf(_r_diff.update(_selector.setpoint().chosen_input.r, dt_s)) > minimum_stick_change); // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] - const bool throttle_moved = (fabsf(_z_diff.diff()) * 2.f) > minimum_stick_change; + const bool throttle_moved = + (fabsf(_z_diff.update(_selector.setpoint().chosen_input.z, dt_s)) * 2.f) > minimum_stick_change; _selector.setpoint().user_override = rpy_moved || throttle_moved; @@ -398,9 +391,7 @@ Module consuming manual_control_inputs publishing one manual_control_setpoint. return 0; } -}; // namespace manual_control - extern "C" __EXPORT int manual_control_main(int argc, char *argv[]) { - return manual_control::ManualControl::main(argc, argv); + return ManualControl::main(argc, argv); } diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index b77b49e4f5..7df2cacf16 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -51,52 +51,10 @@ #include #include #include "ManualControlSelector.hpp" +#include "MovingDiff.hpp" using namespace time_literals; -namespace manual_control -{ - -class MovingDiff -{ -public: - void update(float value, float dt_s) - { - if (!PX4_ISFINITE(value)) { - // Ignore NAN - return; - } - - math::constrain(dt_s, 0.f, _time_period_s); - - // Leave _diff at 0.0f if we don't have a _last_value yet. - if (PX4_ISFINITE(_last_value)) { - const float new_diff = value - _last_value; - _diff = new_diff * dt_s + _diff * (_time_period_s - dt_s); - } - - _last_value = value; - } - - float diff() const - { - return _diff; - } - - void reset() - { - _diff = 0.0f; - _last_value = NAN; - } - -private: - static constexpr float _time_period_s{1.0f}; - - float _diff{0.0f}; - float _last_value{NAN}; -}; - - class ManualControl : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -173,4 +131,3 @@ private: (ParamInt) _param_fltmode_6 ) }; -} // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index b82f5b919f..e736fc13d9 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -33,10 +33,6 @@ #include "ManualControlSelector.hpp" - -namespace manual_control -{ - void ManualControlSelector::updateValidityOfChosenInput(uint64_t now) { if (!isInputValid(_setpoint.chosen_input, now)) { @@ -86,5 +82,3 @@ manual_control_setpoint_s &ManualControlSelector::setpoint() { return _setpoint; } - -} // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index 220c2b7a03..827407f35e 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -37,9 +37,6 @@ #include #include -namespace manual_control -{ - class ManualControlSelector { public: @@ -58,5 +55,3 @@ private: int32_t _rc_in_mode{0}; int _instance{-1}; }; - -} // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp index 12ed384520..8aa0f05c4f 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -36,7 +36,6 @@ #include using namespace time_literals; -using namespace manual_control; static constexpr uint64_t some_time = 12345678; diff --git a/src/modules/manual_control/MovingDiff.hpp b/src/modules/manual_control/MovingDiff.hpp new file mode 100644 index 0000000000..7fa2b8c8be --- /dev/null +++ b/src/modules/manual_control/MovingDiff.hpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +#include + +class MovingDiff +{ +public: + float update(float value, float dt_s) + { + if (!PX4_ISFINITE(value) || (dt_s < FLT_EPSILON)) { + // Ignore NAN + return NAN; + } + + _difference_filter.setParameters(dt_s, .1f); + + // Leave _diff at 0.0f if we don't have a _last_value yet. + if (PX4_ISFINITE(_last_value)) { + const float new_diff = (value - _last_value) / dt_s; + _difference_filter.update(new_diff); + } + + _last_value = value; + return _difference_filter.getState(); + } + + void reset() + { + _difference_filter.reset(0.f); + _last_value = NAN; + } + +private: + AlphaFilter _difference_filter; + float _last_value{NAN}; +};