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

ManualControlSelector: rename functions and validate chosen inputs all the time

Browse Source
This commit is contained in:
Matthias Grob
2021-10-13 14:14:56 +02:00
parent 28a29ad050
commit 0901b73f50
4 changed files with 38 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+5 -10
View File
@@ -84,20 +84,18 @@ void ManualControl::Run()
_stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
_button_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
_selector.set_rc_in_mode(_param_com_rc_in_mode.get());
_selector.set_timeout(_param_com_rc_loss_t.get() * 1_s);
_selector.setRcInMode(_param_com_rc_in_mode.get());
_selector.setTimeout(_param_com_rc_loss_t.get() * 1_s);
}
bool found_at_least_one = false;
const hrt_abstime now = hrt_absolute_time();
const float dt_s = (now - _last_time) / 1e6f;
_selector.updateValidityOfChosenInput(now);
for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) {
manual_control_input_s manual_control_input;
if (_manual_control_input_subs[i].update(&manual_control_input)) {
found_at_least_one = true;
_selector.update_manual_control_input(now, manual_control_input, i);
_selector.updateWithNewInputSample(now, manual_control_input, i);
}
}
@@ -108,10 +106,6 @@ void ManualControl::Run()
switches_updated = true;
}
if (!found_at_least_one) {
_selector.update_time_only(now);
}
if (_selector.setpoint().valid) {
_published_invalid_once = false;
@@ -150,6 +144,7 @@ void ManualControl::Run()
_previous_disarm_gesture = false;
}
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);
src/modules/manual_control/ManualControlSelector.cpp
+4 -4
View File
@@ -37,7 +37,7 @@
namespace manual_control
{
void ManualControlSelector::update_time_only(uint64_t now)
void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
{
if (!isInputValid(_setpoint.chosen_input, now)) {
_setpoint.valid = false;
@@ -45,10 +45,10 @@ void ManualControlSelector::update_time_only(uint64_t now)
}
}
void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance)
void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance)
{
// First check if the setpoint in use got invalid, so it can get replaced below.
update_time_only(now);
// First check if the chosen input got invalid, so it can get replaced
updateValidityOfChosenInput(now);
const bool update_existing_input = _setpoint.valid == true && input.data_source == _setpoint.chosen_input.data_source;
const bool start_using_new_input = _setpoint.valid == false;
src/modules/manual_control/ManualControlSelector.hpp
+4 -4
View File
@@ -43,10 +43,10 @@ namespace manual_control
class ManualControlSelector
{
public:
void set_rc_in_mode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; }
void set_timeout(uint64_t timeout) { _timeout = timeout; }
void update_time_only(uint64_t now);
void update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance);
void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; }
void setTimeout(uint64_t timeout) { _timeout = timeout; }
void updateValidityOfChosenInput(uint64_t now);
void updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance);
manual_control_setpoint_s &setpoint();
int instance() const { return _instance; };
src/modules/manual_control/ManualControlSelectorTest.cpp
+25 -25
View File
@@ -43,8 +43,8 @@ static constexpr uint64_t some_time = 12345678;
TEST(ManualControlSelector, RcInputContinuous)
{
ManualControlSelector selector;
selector.set_rc_in_mode(0);
selector.set_timeout(500_ms);
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
@@ -54,7 +54,7 @@ TEST(ManualControlSelector, RcInputContinuous)
input.timestamp_sample = timestamp;
for (int i = 0; i < 5; i++) {
selector.update_manual_control_input(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().chosen_input.timestamp_sample, timestamp);
EXPECT_EQ(selector.instance(), 1);
@@ -67,15 +67,15 @@ TEST(ManualControlSelector, RcInputContinuous)
TEST(ManualControlSelector, RcInputOnly)
{
ManualControlSelector selector;
selector.set_rc_in_mode(0);
selector.set_timeout(500_ms);
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 0);
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_FALSE(selector.setpoint().valid);
@@ -84,7 +84,7 @@ TEST(ManualControlSelector, RcInputOnly)
// Now provide input with the correct source.
input.data_source = manual_control_input_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
@@ -94,15 +94,15 @@ TEST(ManualControlSelector, RcInputOnly)
TEST(ManualControlSelector, MavlinkInputOnly)
{
ManualControlSelector selector;
selector.set_rc_in_mode(1);
selector.set_timeout(500_ms);
selector.setRcInMode(1);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 0);
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_FALSE(selector.setpoint().valid);
@@ -111,7 +111,7 @@ TEST(ManualControlSelector, MavlinkInputOnly)
// Now provide input with the correct source.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_3;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3);
@@ -122,7 +122,7 @@ TEST(ManualControlSelector, MavlinkInputOnly)
// But only the first MAVLink source wins, others are too late.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_4;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3);
@@ -132,15 +132,15 @@ TEST(ManualControlSelector, MavlinkInputOnly)
TEST(ManualControlSelector, AutoInput)
{
ManualControlSelector selector;
selector.set_rc_in_mode(3);
selector.set_timeout(500_ms);
selector.setRcInMode(3);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 0);
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
@@ -151,7 +151,7 @@ TEST(ManualControlSelector, AutoInput)
// Now provide input from MAVLink as well which should get ignored.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
@@ -162,7 +162,7 @@ TEST(ManualControlSelector, AutoInput)
// Now we'll let RC time out, so it should switch to MAVLINK.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_0);
@@ -172,15 +172,15 @@ TEST(ManualControlSelector, AutoInput)
TEST(ManualControlSelector, RcTimeout)
{
ManualControlSelector selector;
selector.set_rc_in_mode(0);
selector.set_timeout(500_ms);
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 0);
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
@@ -189,7 +189,7 @@ TEST(ManualControlSelector, RcTimeout)
timestamp += 600_ms;
// Update, to make sure it notices the timeout
selector.update_time_only(timestamp);
selector.updateValidityOfChosenInput(timestamp);
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_EQ(selector.instance(), -1);
@@ -198,21 +198,21 @@ TEST(ManualControlSelector, RcTimeout)
TEST(ManualControlSelector, RcOutdated)
{
ManualControlSelector selector;
selector.set_rc_in_mode(0);
selector.set_timeout(500_ms);
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated
selector.update_manual_control_input(timestamp, input, 0);
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_EQ(selector.instance(), -1);
// If we update again it should still not get accepted
selector.update_manual_control_input(timestamp, input, 0);
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_EQ(selector.instance(), -1);