fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 05:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ManualControl: fix RC override

Browse Source
This commit is contained in:
Matthias Grob
2021-11-02 15:29:33 +01:00
parent 7e4a079e09
commit 423aadcc4f
6 changed files with 76 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+6 -15
View File
@@ -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)
@@ -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);
}