dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

manual_control: move override detection 

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-04-15 14:28:52 +02:00 committed by Matthias Grob
parent 1c15cc11d8
commit cda6524421
5 changed files with 27 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/manual_control_setpoint.msg
View File

@ -43,12 +43,6 @@ float32 r # yaw stick/twist position, -1..1
# in general corresponds to the righthand rotation around the vertical
# (downwards) axis of the vehicle
# stick velocity
float32 vx
float32 vy
float32 vz
float32 vr
float32 flaps # flap position
float32 aux1

20
src/modules/commander/ManualControl.cpp
View File

@ -36,10 +36,9 @@
using namespace time_literals;
enum OverrideBits {
enum class OverrideBits : int32_t {
OVERRIDE_AUTO_MODE_BIT = (1 << 0),
OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
};
bool ManualControl::update()
@ -63,10 +62,10 @@ bool ManualControl::update()
bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode,
const vehicle_status_s &vehicle_status)
{
const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
const bool override_auto_mode = (_param_rc_override.get() & static_cast<int32_t>(OverrideBits::OVERRIDE_AUTO_MODE_BIT))
&& vehicle_control_mode.flag_control_auto_enabled;
const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
const bool override_offboard_mode = (_param_rc_override.get() & static_cast<int32_t>(OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT))
&& vehicle_control_mode.flag_control_offboard_enabled;
// in Descend manual override is enbaled independently of COM_RC_OVERRIDE
@ -74,19 +73,8 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {
const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.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(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
if (rpy_moved || (use_throttle && throttle_moved)) {
return true;
}
return _manual_control_setpoint.user_override;
}
return false;

3
src/modules/commander/commander_params.c
View File

@ -669,10 +669,9 @@ PARAM_DEFINE_INT32(COM_REARM_GRACE, 1);
* override is always enabled.
*
* @min 0
* @max 7
* @max 3
* @bit 0 Enable override during auto modes (except for in critical battery reaction)
* @bit 1 Enable override during offboard mode
* @bit 2 Ignore throttle stick
* @group Commander
*/
PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);

26
src/modules/manual_control/ManualControl.cpp
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

13
src/modules/manual_control/ManualControl.hpp
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
)