Skip to content

Commit

Permalink
ManualControl: fix RC override
Browse files Browse the repository at this point in the history
  • Loading branch information
MaEtUgR committed Nov 8, 2021
1 parent 9293c03 commit 0f82b49
Show file tree
Hide file tree
Showing 6 changed files with 76 additions and 71 deletions.
21 changes: 6 additions & 15 deletions src/modules/manual_control/ManualControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,6 @@
#include "ManualControl.hpp"
#include <uORB/topics/commander_state.h>

namespace manual_control
{

ManualControl::ManualControl() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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);
}
45 changes: 1 addition & 44 deletions src/modules/manual_control/ManualControl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,52 +51,10 @@
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
#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<ManualControl>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
Expand Down Expand Up @@ -173,4 +131,3 @@ class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, pub
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
)
};
} // namespace manual_control
6 changes: 0 additions & 6 deletions src/modules/manual_control/ManualControlSelector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,6 @@

#include "ManualControlSelector.hpp"


namespace manual_control
{

void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
{
if (!isInputValid(_setpoint.chosen_input, now)) {
Expand Down Expand Up @@ -86,5 +82,3 @@ manual_control_setpoint_s &ManualControlSelector::setpoint()
{
return _setpoint;
}

} // namespace manual_control
5 changes: 0 additions & 5 deletions src/modules/manual_control/ManualControlSelector.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,6 @@
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_setpoint.h>

namespace manual_control
{

class ManualControlSelector
{
public:
Expand All @@ -58,5 +55,3 @@ class ManualControlSelector
int32_t _rc_in_mode{0};
int _instance{-1};
};

} // namespace manual_control
1 change: 0 additions & 1 deletion src/modules/manual_control/ManualControlSelectorTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@
#include <drivers/drv_hrt.h>

using namespace time_literals;
using namespace manual_control;

static constexpr uint64_t some_time = 12345678;

Expand Down
69 changes: 69 additions & 0 deletions src/modules/manual_control/MovingDiff.hpp
Original file line number Diff line number Diff line change
@@ -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 <lib/mathlib/math/filter/AlphaFilter.hpp>

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<float> _difference_filter;
float _last_value{NAN};
};

0 comments on commit 0f82b49

Please sign in to comment.