fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 16:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

manual_control: move override detection

Browse Source 
This also removes the option to ignore throttle for the override
detection as it's not really required anymore.
This commit is contained in:
Julian Oes
2021-11-09 16:05:25 +01:00
committed by Matthias Grob
parent 1c15cc11d8
commit cda6524421
5 changed files with 27 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+17 -9
View File
@@ -137,20 +137,23 @@ void ManualControl::Run()
}
// user wants override
//const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
// FIXME: we need previous
//const bool rpy_moved = (fabsf(_selector.setpoint().x - _manual_control_input[i].x) > minimum_stick_change)
// || (fabsf(_selector.setpoint().y - _manual_control_input[i].y) > minimum_stick_change)
// || (fabsf(_selector.setpoint().r - _manual_control_input[i].r) > minimum_stick_change);
// TODO: look at least at 3 samples in a specific time
const bool rpy_moved = (fabsf(_selector.setpoint().x - _previous_x) > minimum_stick_change)
|| (fabsf(_selector.setpoint().y - _previous_y) > minimum_stick_change)
|| (fabsf(_selector.setpoint().r - _previous_r) > 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(_selector.setpoint().z - _manual_control_input[i].z) * 2.f > minimum_stick_change);
//const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
const bool throttle_moved = (fabsf(_selector.setpoint().z - _previous_z) * 2.f > minimum_stick_change);
//_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved);
_selector.setpoint().user_override = rpy_moved || throttle_moved;
// TODO: at least 3 samples in a time
_previous_x = _selector.setpoint().x;
_previous_y = _selector.setpoint().y;
_previous_z = _selector.setpoint().z;
_previous_r = _selector.setpoint().r;
if (switches_updated) {
// Only use switches if current source is RC as well.
@@ -177,6 +180,11 @@ void ManualControl::Run()
_published_invalid_once = true;
_manual_control_setpoint_pub.publish(_selector.setpoint());
}
_previous_x = NAN;
_previous_y = NAN;
_previous_z = NAN;
_previous_r = NAN;
}
// reschedule timeout
src/modules/manual_control/ManualControl.hpp
+5 -8
View File
@@ -76,13 +76,6 @@ public:
private:
static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
enum OverrideBits {
OVERRIDE_AUTO_MODE_BIT = (1 << 0),
OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
};
void Run() override;
void send_arm_command();
@@ -108,13 +101,17 @@ private:
bool _previous_arm_gesture{false};
bool _previous_disarm_gesture{false};
float _previous_x{NAN};
float _previous_y{NAN};
float _previous_z{NAN};
float _previous_r{NAN};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst
)