Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ManualControl: separate function for stick arming

Browse Source
This commit is contained in:
Matthias Grob
2021-10-20 19:33:22 +02:00
parent a349dae760
commit e73e6a490d
2 changed files with 27 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+25 -23
View File
@@ -109,29 +109,7 @@ void ManualControl::Run()
if (_selector.setpoint().valid) {
_published_invalid_once = false;
// Arm gesture
const bool right_stick_centered = (fabsf(_selector.setpoint().chosen_input.x) < 0.1f)
&& (fabsf(_selector.setpoint().chosen_input.y) < 0.1f);
const bool stick_lower_right = (_selector.setpoint().chosen_input.z < 0.1f)
&& (_selector.setpoint().chosen_input.r > 0.9f);
const bool previous_stick_arm_hysteresis = _stick_arm_hysteresis.get_state();
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp);
if (!previous_stick_arm_hysteresis && _stick_arm_hysteresis.get_state()) {
sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_STICK_GESTURE);
}
// Disarm gesture
const bool stick_lower_left = (_selector.setpoint().chosen_input.z < 0.1f)
&& (_selector.setpoint().chosen_input.r < -0.9f);
const bool previous_stick_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp);
if (!previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) {
sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_STICK_GESTURE);
}
processStickArming(_selector.setpoint().chosen_input);
// User override by stick
const float dt_s = (now - _last_time) / 1e6f;
@@ -287,6 +265,30 @@ void ManualControl::Run()
perf_end(_loop_perf);
}
void ManualControl::processStickArming(const manual_control_input_s &input)
{
// Arm gesture
const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f);
const bool stick_lower_right = (input.z < 0.1f) && (input.r > 0.9f);
const bool previous_stick_arm_hysteresis = _stick_arm_hysteresis.get_state();
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, input.timestamp);
if (!previous_stick_arm_hysteresis && _stick_arm_hysteresis.get_state()) {
sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_STICK_GESTURE);
}
// Disarm gesture
const bool stick_lower_left = (input.z < 0.1f) && (input.r < -0.9f);
const bool previous_stick_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, input.timestamp);
if (!previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) {
sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_STICK_GESTURE);
}
}
void ManualControl::evaluateModeSlot(uint8_t mode_slot)
{
switch (mode_slot) {
src/modules/manual_control/ManualControl.hpp
+2 -1
View File
@@ -46,7 +46,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include "ManualControlSelector.hpp"
@@ -118,6 +118,7 @@ private:
static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
void Run() override;
void processStickArming(const manual_control_input_s &input);
void evaluateModeSlot(uint8_t mode_slot);
void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0);