forked from Archive/PX4-Autopilot
manual_control: add selector class [WIP]
This adds a selector class with unit tests. The idea is to have a valid flag in manual_control_septoint and set that according to the selection and/or timeout of manual_control_inputs.
This commit is contained in:
Julian Oes
Matthias Grob
parent
eeb7682fd9
commit
723db8bf2a
|
|
@ -2,6 +2,8 @@ uint64 timestamp # time since system start (microseconds)
|
|||
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
bool valid
|
||||
|
||||
uint8 SOURCE_UNKNOWN = 0
|
||||
uint8 SOURCE_RC = 1 # radio control (input_rc)
|
||||
uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0
|
||||
|
|
|
|||
|
|
@ -39,6 +39,10 @@ px4_add_module(
|
|||
ManualControl.cpp
|
||||
ManualControl.cpp
|
||||
ManualControl.hpp
|
||||
ManualControlSelector.hpp
|
||||
ManualControlSelector.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
px4_add_unit_gtest(SRC ManualControlSelectorTest.cpp LINKLIBS module__manual_control)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -77,109 +77,77 @@ void ManualControl::Run()
|
|||
|
||||
_stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
|
||||
_stick_disarm_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);
|
||||
}
|
||||
|
||||
const hrt_abstime rc_timeout = _param_com_rc_loss_t.get() * 1_s;
|
||||
|
||||
bool publish_once = false;
|
||||
int selected_manual_input = -1;
|
||||
bool found_at_least_one = false;
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
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)) {
|
||||
|
||||
bool publish = false;
|
||||
found_at_least_one = true;
|
||||
|
||||
if ((_param_com_rc_in_mode.get() == 0)
|
||||
&& (manual_control_input.data_source == manual_control_input_s::SOURCE_RC)) {
|
||||
_selector.update_manual_control_input(now, manual_control_input);
|
||||
|
||||
publish = true;
|
||||
|
||||
} else if ((_param_com_rc_in_mode.get() == 1)
|
||||
&& (manual_control_input.data_source >= manual_control_input_s::SOURCE_MAVLINK_0)) {
|
||||
|
||||
publish = true;
|
||||
|
||||
} else {
|
||||
// otherwise, first come, first serve (REVIEW)
|
||||
publish = true;
|
||||
}
|
||||
|
||||
|
||||
bool available = (hrt_elapsed_time(&manual_control_input.timestamp) < rc_timeout)
|
||||
&& (hrt_elapsed_time(&_manual_control_input[i].timestamp) < rc_timeout);
|
||||
|
||||
if (publish && available && !publish_once) {
|
||||
const float dt_inv = 1e6f / static_cast<float>(manual_control_input.timestamp_sample -
|
||||
_manual_control_input[i].timestamp_sample);
|
||||
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
manual_control_setpoint.timestamp_sample = manual_control_input.timestamp_sample;
|
||||
manual_control_setpoint.x = manual_control_input.x;
|
||||
manual_control_setpoint.y = manual_control_input.y;
|
||||
manual_control_setpoint.z = manual_control_input.z;
|
||||
manual_control_setpoint.r = manual_control_input.r;
|
||||
manual_control_setpoint.vx = (manual_control_input.x - _manual_control_input[i].x) * dt_inv;
|
||||
manual_control_setpoint.vy = (manual_control_input.y - _manual_control_input[i].y) * dt_inv;
|
||||
manual_control_setpoint.vz = (manual_control_input.z - _manual_control_input[i].z) * dt_inv;
|
||||
manual_control_setpoint.vr = (manual_control_input.r - _manual_control_input[i].r) * dt_inv;
|
||||
manual_control_setpoint.flaps = manual_control_input.flaps;
|
||||
manual_control_setpoint.aux1 = manual_control_input.aux1;
|
||||
manual_control_setpoint.aux2 = manual_control_input.aux2;
|
||||
manual_control_setpoint.aux3 = manual_control_input.aux3;
|
||||
manual_control_setpoint.aux4 = manual_control_input.aux4;
|
||||
manual_control_setpoint.aux5 = manual_control_input.aux5;
|
||||
manual_control_setpoint.aux6 = manual_control_input.aux6;
|
||||
manual_control_setpoint.data_source = manual_control_input.data_source;
|
||||
|
||||
|
||||
// user arm/disarm gesture
|
||||
const bool right_stick_centered = (fabsf(manual_control_input.x) < 0.1f) && (fabsf(manual_control_input.y) < 0.1f);
|
||||
const bool stick_lower_left = (manual_control_input.z < 0.1f) && (manual_control_input.r < -0.9f);
|
||||
const bool stick_lower_right = (manual_control_input.z < 0.1f) && (manual_control_input.r > 0.9f);
|
||||
|
||||
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, manual_control_input.timestamp);
|
||||
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, manual_control_input.timestamp);
|
||||
manual_control_setpoint.arm_gesture = _stick_arm_hysteresis.get_state();
|
||||
manual_control_setpoint.disarm_gesture = _stick_disarm_hysteresis.get_state();
|
||||
|
||||
|
||||
// user wants override
|
||||
const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
|
||||
const bool rpy_moved = (fabsf(manual_control_input.x - _manual_control_input[i].x) > minimum_stick_change)
|
||||
|| (fabsf(manual_control_input.y - _manual_control_input[i].y) > minimum_stick_change)
|
||||
|| (fabsf(manual_control_input.r - _manual_control_input[i].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_input.z - _manual_control_input[i].z) * 2.f > minimum_stick_change);
|
||||
const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
|
||||
|
||||
manual_control_setpoint.user_override = rpy_moved || (use_throttle && throttle_moved);
|
||||
|
||||
|
||||
manual_control_setpoint.timestamp = hrt_absolute_time();
|
||||
_manual_control_setpoint_pub.publish(manual_control_setpoint);
|
||||
publish_once = true;
|
||||
|
||||
selected_manual_input = i;
|
||||
}
|
||||
|
||||
_manual_control_input[i] = manual_control_input;
|
||||
_available[i] = available;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
if (!found_at_least_one) {
|
||||
_selector.update_time_only(now);
|
||||
}
|
||||
|
||||
if (_selector.setpoint().valid) {
|
||||
_published_invalid_once = false;
|
||||
|
||||
// user arm/disarm gesture
|
||||
const bool right_stick_centered = (fabsf(_selector.setpoint().x) < 0.1f) && (fabsf(_selector.setpoint().y) < 0.1f);
|
||||
const bool stick_lower_left = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r < -0.9f);
|
||||
const bool stick_lower_right = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r > 0.9f);
|
||||
|
||||
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp);
|
||||
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp);
|
||||
_selector.setpoint().arm_gesture = _stick_arm_hysteresis.get_state();
|
||||
_selector.setpoint().disarm_gesture = _stick_disarm_hysteresis.get_state();
|
||||
|
||||
|
||||
// user wants override
|
||||
//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);
|
||||
|
||||
// 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);
|
||||
|
||||
//_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved);
|
||||
|
||||
|
||||
_selector.setpoint().timestamp = now;
|
||||
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
||||
|
||||
} else {
|
||||
_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);
|
||||
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include "ManualControlSelector.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
|
@ -92,15 +93,12 @@ private:
|
|||
{this, ORB_ID(manual_control_input), 2},
|
||||
};
|
||||
|
||||
manual_control_input_s _manual_control_input[MAX_MANUAL_INPUT_COUNT] {};
|
||||
|
||||
bool _available[MAX_MANUAL_INPUT_COUNT] {};
|
||||
|
||||
int8_t _selected_manual_input{-1};
|
||||
|
||||
systemlib::Hysteresis _stick_arm_hysteresis{false};
|
||||
systemlib::Hysteresis _stick_disarm_hysteresis{false};
|
||||
|
||||
ManualControlSelector _selector;
|
||||
bool _published_invalid_once{false};
|
||||
|
||||
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")};
|
||||
|
||||
|
|
@ -112,4 +110,4 @@ private:
|
|||
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst
|
||||
)
|
||||
};
|
||||
}; // namespace manual_control
|
||||
} // namespace manual_control
|
||||
|
|
|
|||
|
|
@ -0,0 +1,115 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ManualControlSelector.hpp"
|
||||
|
||||
|
||||
namespace manual_control
|
||||
{
|
||||
|
||||
void ManualControlSelector::update_time_only(uint64_t now)
|
||||
{
|
||||
if (_setpoint.timestamp_sample + _timeout < now) {
|
||||
_setpoint.valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input)
|
||||
{
|
||||
// This method requires the current timestamp explicitly in order to prevent weird cases
|
||||
// if the timestamp_sample of some source is invalid/wrong.
|
||||
|
||||
// If previous setpoint is timed out, set it invalid, so it can get replaced below.
|
||||
update_time_only(now);
|
||||
|
||||
if (_rc_in_mode == 0 && input.data_source == manual_control_input_s::SOURCE_RC) {
|
||||
_setpoint = setpoint_from_input(input);
|
||||
_setpoint.valid = true;
|
||||
|
||||
} 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
|
||||
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_2
|
||||
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_3
|
||||
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_4
|
||||
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_5)) {
|
||||
|
||||
// We only stick to the first discovered mavlink channel.
|
||||
if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
|
||||
_setpoint = setpoint_from_input(input);
|
||||
_setpoint.valid = true;
|
||||
}
|
||||
} else if (_rc_in_mode == 2) {
|
||||
// FIXME: what to do in the legacy case?
|
||||
} else if (_rc_in_mode == 3) {
|
||||
|
||||
// We only stick to the first discovered mavlink channel.
|
||||
if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
|
||||
_setpoint = setpoint_from_input(input);
|
||||
_setpoint.valid = true;
|
||||
}
|
||||
} else {
|
||||
// FIXME: param value unknown, what to do?
|
||||
}
|
||||
}
|
||||
|
||||
manual_control_setpoint_s ManualControlSelector::setpoint_from_input(const manual_control_input_s &input)
|
||||
{
|
||||
manual_control_setpoint_s setpoint;
|
||||
setpoint.timestamp_sample = input.timestamp_sample;
|
||||
setpoint.x = input.x;
|
||||
setpoint.y = input.y;
|
||||
setpoint.z = input.z;
|
||||
setpoint.r = input.r;
|
||||
// FIXME: what's that?
|
||||
//setpoint.vx = (input.x - _manual_control_input[i].x) * dt_inv;
|
||||
//setpoint.vy = (input.y - _manual_control_input[i].y) * dt_inv;
|
||||
//setpoint.vz = (input.z - _manual_control_input[i].z) * dt_inv;
|
||||
//setpoint.vr = (input.r - _manual_control_input[i].r) * dt_inv;
|
||||
setpoint.flaps = input.flaps;
|
||||
setpoint.aux1 = input.aux1;
|
||||
setpoint.aux2 = input.aux2;
|
||||
setpoint.aux3 = input.aux3;
|
||||
setpoint.aux4 = input.aux4;
|
||||
setpoint.aux5 = input.aux5;
|
||||
setpoint.aux6 = input.aux6;
|
||||
setpoint.data_source = input.data_source;
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
|
||||
manual_control_setpoint_s &ManualControlSelector::setpoint()
|
||||
{
|
||||
return _setpoint;
|
||||
}
|
||||
|
||||
} // namespace manual_control
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <uORB/topics/manual_control_input.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
|
||||
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);
|
||||
manual_control_setpoint_s &setpoint();
|
||||
|
||||
private:
|
||||
manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input);
|
||||
|
||||
manual_control_setpoint_s _setpoint {};
|
||||
uint64_t _timeout {0};
|
||||
int32_t _rc_in_mode {0};
|
||||
};
|
||||
|
||||
} // namespace manual_control
|
||||
|
|
@ -0,0 +1,167 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ManualControlSelector.hpp"
|
||||
#include <gtest/gtest.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace manual_control;
|
||||
|
||||
static constexpr uint64_t some_time = 1234568;
|
||||
|
||||
|
||||
TEST(ManualControlSelector, RcInputOnly)
|
||||
{
|
||||
ManualControlSelector selector;
|
||||
selector.set_rc_in_mode(0);
|
||||
selector.set_timeout(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);
|
||||
|
||||
EXPECT_FALSE(selector.setpoint().valid);
|
||||
|
||||
timestamp += 100_ms;
|
||||
|
||||
// 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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
|
||||
}
|
||||
|
||||
TEST(ManualControlSelector, MavlinkInputOnly)
|
||||
{
|
||||
ManualControlSelector selector;
|
||||
selector.set_rc_in_mode(1);
|
||||
selector.set_timeout(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);
|
||||
|
||||
EXPECT_FALSE(selector.setpoint().valid);
|
||||
|
||||
timestamp += 100_ms;
|
||||
|
||||
// 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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
|
||||
|
||||
// But only the first MAVLink source wins, others are too late.
|
||||
|
||||
timestamp += 100_ms;
|
||||
|
||||
// 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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
|
||||
}
|
||||
|
||||
TEST(ManualControlSelector, AutoInput)
|
||||
{
|
||||
ManualControlSelector selector;
|
||||
selector.set_rc_in_mode(3);
|
||||
selector.set_timeout(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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
|
||||
|
||||
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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
|
||||
|
||||
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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
|
||||
}
|
||||
|
||||
TEST(ManualControlSelector, RcTimeout)
|
||||
{
|
||||
ManualControlSelector selector;
|
||||
selector.set_rc_in_mode(0);
|
||||
selector.set_timeout(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);
|
||||
|
||||
EXPECT_TRUE(selector.setpoint().valid);
|
||||
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
|
||||
|
||||
timestamp += 600_ms;
|
||||
|
||||
// Update, to make sure it notices the timeout
|
||||
selector.update_time_only(timestamp);
|
||||
|
||||
EXPECT_FALSE(selector.setpoint().valid);
|
||||
}
|
||||
Loading…
Reference in New Issue