fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use backwards compatible manual_control_setpoint instead of manual_control_input

Browse Source
This commit is contained in:
Matthias Grob
2021-11-09 16:05:25 +01:00
parent 423aadcc4f
commit fabf865411
30 changed files with 252 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+13 -13
View File
@@ -83,9 +83,9 @@ void ManualControl::Run()
_selector.updateValidityOfChosenInput(now);
for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) {
manual_control_input_s manual_control_input;
manual_control_setpoint_s manual_control_input;
if (_manual_control_input_subs[i].update(&manual_control_input)) {
if (_manual_control_setpoint_subs[i].update(&manual_control_input)) {
_selector.updateWithNewInputSample(now, manual_control_input, i);
}
}
@@ -96,25 +96,25 @@ void ManualControl::Run()
if (_selector.setpoint().valid) {
_published_invalid_once = false;
processStickArming(_selector.setpoint().chosen_input);
processStickArming(_selector.setpoint());
// User override by stick
const float dt_s = (now - _last_time) / 1e6f;
const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
const bool rpy_moved = (fabsf(_x_diff.update(_selector.setpoint().chosen_input.x, dt_s)) > minimum_stick_change)
|| (fabsf(_y_diff.update(_selector.setpoint().chosen_input.y, dt_s)) > minimum_stick_change)
|| (fabsf(_r_diff.update(_selector.setpoint().chosen_input.r, dt_s)) > minimum_stick_change);
const bool rpy_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change)
|| (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change)
|| (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > 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(_z_diff.update(_selector.setpoint().chosen_input.z, dt_s)) * 2.f) > minimum_stick_change;
const bool throttle_moving =
(fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) * 2.f) > minimum_stick_change;
_selector.setpoint().user_override = rpy_moved || throttle_moved;
_selector.setpoint().sticks_moving = rpy_moving || throttle_moving;
if (switches_updated) {
// Only use switches if current source is RC as well.
if (_selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC) {
if (_selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC) {
if (_previous_switches_initialized) {
if (switches.mode_slot != _previous_switches.mode_slot) {
evaluateModeSlot(switches.mode_slot);
@@ -223,11 +223,11 @@ void ManualControl::Run()
if (instance != _previous_manual_control_input_instance) {
if ((0 <= _previous_manual_control_input_instance)
&& (_previous_manual_control_input_instance < MAX_MANUAL_INPUT_COUNT)) {
_manual_control_input_subs[_previous_manual_control_input_instance].unregisterCallback();
_manual_control_setpoint_subs[_previous_manual_control_input_instance].unregisterCallback();
}
if ((0 <= instance) && (instance < MAX_MANUAL_INPUT_COUNT)) {
_manual_control_input_subs[instance].registerCallback();
_manual_control_setpoint_subs[instance].registerCallback();
}
_previous_manual_control_input_instance = instance;
@@ -258,7 +258,7 @@ void ManualControl::Run()
perf_end(_loop_perf);
}
void ManualControl::processStickArming(const manual_control_input_s &input)
void ManualControl::processStickArming(const manual_control_setpoint_s &input)
{
// Arm gesture
const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f);
src/modules/manual_control/ManualControl.hpp
+2 -3
View File
@@ -43,7 +43,6 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/topics/action_request.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
@@ -78,7 +77,7 @@ private:
static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
void Run() override;
void processStickArming(const manual_control_input_s &input);
void processStickArming(const manual_control_setpoint_s &input);
void evaluateModeSlot(uint8_t mode_slot);
void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0);
@@ -90,7 +89,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int _previous_manual_control_input_instance{-1};
uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] {
uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] {
{this, ORB_ID(manual_control_input), 0},
{this, ORB_ID(manual_control_input), 1},
{this, ORB_ID(manual_control_input), 2},
src/modules/manual_control/ManualControlSelector.cpp
+12 -12
View File
@@ -35,43 +35,43 @@
void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
{
if (!isInputValid(_setpoint.chosen_input, now)) {
if (!isInputValid(_setpoint, now)) {
_setpoint.valid = false;
_instance = -1;
}
}
void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance)
void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance)
{
// First check if the chosen input got invalid, so it can get replaced
updateValidityOfChosenInput(now);
const bool update_existing_input = _setpoint.valid && input.data_source == _setpoint.chosen_input.data_source;
const bool update_existing_input = _setpoint.valid && input.data_source == _setpoint.data_source;
const bool start_using_new_input = !_setpoint.valid;
// Switch to new input if it's valid and we don't already have a valid one
if (isInputValid(input, now) && (update_existing_input || start_using_new_input)) {
_setpoint.chosen_input = input;
_setpoint = input;
_setpoint.valid = true;
_instance = instance;
}
}
bool ManualControlSelector::isInputValid(const manual_control_input_s &input, uint64_t now) const
bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input, uint64_t now) const
{
// Check for timeout
const bool sample_from_the_past = now >= input.timestamp_sample;
const bool sample_newer_than_timeout = now - input.timestamp_sample < _timeout;
// Check if source matches the configuration
const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_input_s::SOURCE_RC);
const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_setpoint_s::SOURCE_RC);
const bool source_mavlink_matched = (_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);
(input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_1
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_2
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5);
const bool source_any_matched = (_rc_in_mode == 3);
return sample_from_the_past && sample_newer_than_timeout
src/modules/manual_control/ManualControlSelector.hpp
+2 -3
View File
@@ -34,7 +34,6 @@
#pragma once
#include <stdint.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_setpoint.h>
class ManualControlSelector
@@ -43,12 +42,12 @@ public:
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);
void updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance);
manual_control_setpoint_s &setpoint();
int instance() const { return _instance; };
private:
bool isInputValid(const manual_control_input_s &input, uint64_t now) const;
bool isInputValid(const manual_control_setpoint_s &input, uint64_t now) const;
manual_control_setpoint_s _setpoint{};
uint64_t _timeout{0};
src/modules/manual_control/ManualControlSelectorTest.cpp
+26 -26
View File
@@ -48,16 +48,16 @@ TEST(ManualControlSelector, RcInputContinuous)
uint64_t timestamp = some_time;
// Now provide input with the correct source.
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
for (int i = 0; i < 5; i++) {
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().chosen_input.timestamp_sample, timestamp);
EXPECT_EQ(selector.setpoint().timestamp_sample, timestamp);
EXPECT_EQ(selector.instance(), 1);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
timestamp += 100_ms;
input.timestamp_sample = timestamp;
}
@@ -71,8 +71,8 @@ TEST(ManualControlSelector, RcInputOnly)
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -81,12 +81,12 @@ TEST(ManualControlSelector, RcInputOnly)
timestamp += 100_ms;
// Now provide input with the correct source.
input.data_source = manual_control_input_s::SOURCE_RC;
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.instance(), 1);
}
@@ -98,8 +98,8 @@ TEST(ManualControlSelector, MavlinkInputOnly)
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -108,23 +108,23 @@ TEST(ManualControlSelector, MavlinkInputOnly)
timestamp += 100_ms;
// Now provide input with the correct source.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_3;
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_3;
input.timestamp_sample = timestamp;
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);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1);
timestamp += 100_ms;
// But only the first MAVLink source wins, others are too late.
input.data_source = manual_control_input_s::SOURCE_MAVLINK_4;
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_4;
input.timestamp_sample = timestamp;
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);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1);
}
@@ -136,35 +136,35 @@ TEST(ManualControlSelector, AutoInput)
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
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.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
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.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
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);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
}
@@ -176,13 +176,13 @@ TEST(ManualControlSelector, RcTimeout)
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 600_ms;
@@ -202,8 +202,8 @@ TEST(ManualControlSelector, RcOutdated)
uint64_t timestamp = some_time;
manual_control_input_s input {};
input.data_source = manual_control_input_s::SOURCE_RC;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated
selector.updateWithNewInputSample(timestamp, input, 0);
src/modules/manual_control/MovingDiffTest.cpp
+49
View File
@@ -0,0 +1,49 @@
/****************************************************************************
*
* 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 "MovingDiff.hpp"
#include <gtest/gtest.h>
TEST(MovingDiffTest, RcInputContinuous)
{
MovingDiff _diff;
EXPECT_FLOAT_EQ(_diff.update(0.0f, 0.0f), 0.f); // 0,0,0
EXPECT_FLOAT_EQ(_diff.update(1.0f, 1.0f), 0.f); // 1*,0,0
EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 1,-1*,0
EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 0,-1,0*
EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 0*,-1,0
EXPECT_FLOAT_EQ(_diff.update(1.0f, 1.0f), 0.f); // 0,1*,0
EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 0,1,-1*
EXPECT_FLOAT_EQ(_diff.update(2.0f, 1.0f), 1.f); // 2*,1,-1
EXPECT_FLOAT_EQ(_diff.update(4.0f, 1.0f), 2.f); // 2,2*,-1
}