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

manual_control: add instance to selector 

This instance is then used to schedule the callback for the
subscription.
This commit is contained in:
Julian Oes 2021-04-14 11:22:29 +02:00 committed by Matthias Grob
parent 14eb656a59
commit 8876af9150
5 changed files with 42 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/manual_control/ManualControl.cpp
View File

@ -92,7 +92,7 @@ void ManualControl::Run()
found_at_least_one = true;
_selector.update_manual_control_input(now, manual_control_input);
_selector.update_manual_control_input(now, manual_control_input, i);
}
}
@ -133,20 +133,22 @@ void ManualControl::Run()
_selector.setpoint().timestamp = now;
_manual_control_setpoint_pub.publish(_selector.setpoint());
_manual_control_input_subs[_selector.instance()].registerCallback();
if (_last_selected_input != _selector.instance()) {
PX4_INFO("selected manual_control_input changed %d -> %d", _last_selected_input, _selector.instance());
_last_selected_input = _selector.instance();
}
} else {
_last_selected_input = -1;
if (!_published_invalid_once) {
_published_invalid_once = true;
_manual_control_setpoint_pub.publish(_selector.setpoint());
}
// FIXME: get from selector
//if ((selected_manual_input >= 0) && (selected_manual_input != _selected_manual_input)) {
// if (_selected_manual_input >= 0) {
// PX4_INFO("selected manual_control_input changed %d -> %d", _selected_manual_input, selected_manual_input);
// }
// _manual_control_input_subs[selected_manual_input].registerCallback();
// _selected_manual_input = selected_manual_input;
//}
}
// reschedule timeout
ScheduleDelayed(200_ms);
@ -198,7 +200,7 @@ int ManualControl::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Module consuming manual_control_inputs publishing one manual_control_setpoint.
)DESCR_STR");

1
src/modules/manual_control/ManualControl.hpp
View File

@ -98,6 +98,7 @@ private:
ManualControlSelector _selector;
bool _published_invalid_once{false};
int _last_selected_input{-1};
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")};

6
src/modules/manual_control/ManualControlSelector.cpp
View File

@ -41,10 +41,11 @@ void ManualControlSelector::update_time_only(uint64_t now)
{
if (_setpoint.timestamp_sample + _timeout < now) {
_setpoint.valid = false;
_instance = -1;
}
}
void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input)
void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance)
{
// This method requires the current timestamp explicitly in order to prevent weird cases
// if the timestamp_sample of some source is invalid/wrong.
@ -55,6 +56,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
if (_rc_in_mode == 0 && input.data_source == manual_control_input_s::SOURCE_RC) {
_setpoint = setpoint_from_input(input);
_setpoint.valid = true;
_instance = instance;
} else if (_rc_in_mode == 1 && (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_1
@ -67,6 +69,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
_setpoint = setpoint_from_input(input);
_setpoint.valid = true;
_instance = instance;
}
} else if (_rc_in_mode == 2) {
// FIXME: what to do in the legacy case?
@ -76,6 +79,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
_setpoint = setpoint_from_input(input);
_setpoint.valid = true;
_instance = instance;
}
} else {
// FIXME: param value unknown, what to do?

4
src/modules/manual_control/ManualControlSelector.hpp
View File

@ -45,8 +45,9 @@ 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);
void update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance);
manual_control_setpoint_s &setpoint();
int instance() const { return _instance; };
private:
manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input);
@ -54,6 +55,7 @@ private:
manual_control_setpoint_s _setpoint {};
uint64_t _timeout {0};
int32_t _rc_in_mode {0};
int _instance {-1};
};
} // namespace manual_control

26
src/modules/manual_control/ManualControlSelectorTest.cpp
View File

@ -52,7 +52,7 @@ TEST(ManualControlSelector, RcInputOnly)
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);
selector.update_manual_control_input(timestamp, input, 0);
EXPECT_FALSE(selector.setpoint().valid);
@ -61,10 +61,11 @@ 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);
selector.update_manual_control_input(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, MavlinkInputOnly)
@ -78,7 +79,7 @@ TEST(ManualControlSelector, MavlinkInputOnly)
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);
selector.update_manual_control_input(timestamp, input, 0);
EXPECT_FALSE(selector.setpoint().valid);
@ -87,10 +88,11 @@ 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);
selector.update_manual_control_input(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1);
// But only the first MAVLink source wins, others are too late.
@ -99,10 +101,11 @@ TEST(ManualControlSelector, MavlinkInputOnly)
// Now provide input with the correct source.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_4;
input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input);
selector.update_manual_control_input(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, AutoInput)
@ -116,30 +119,33 @@ TEST(ManualControlSelector, AutoInput)
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);
selector.update_manual_control_input(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 100_ms;
// 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);
selector.update_manual_control_input(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// 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);
selector.update_manual_control_input(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, RcTimeout)
@ -153,10 +159,11 @@ TEST(ManualControlSelector, RcTimeout)
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);
selector.update_manual_control_input(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 600_ms;
@ -164,4 +171,5 @@ TEST(ManualControlSelector, RcTimeout)
selector.update_time_only(timestamp);
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_EQ(selector.instance(), -1);
}