Browse Source
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.master
Julian Oes
4 years ago
committed by
Matthias Grob
7 changed files with 398 additions and 85 deletions
@ -0,0 +1,115 @@
@@ -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 @@
@@ -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 @@
@@ -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