fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: extend COM_RC_IN_MODE 5 and 6 to source ID ascending and descending priority modes

Browse Source
This commit is contained in:
Tobias Büchli 2025-09-23 23:18:50 +02:00
parent 6c69d86d7e
commit 25c66a7ee5
6 changed files with 114 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp
View File

@ -75,7 +75,8 @@ void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporte
return;
}
if (_param_com_rc_in_mode.get() != 0 && _param_com_rc_in_mode.get() != 2 && _param_com_rc_in_mode.get() != 3) {
if (_param_com_rc_in_mode.get() != 0 && _param_com_rc_in_mode.get() != 2 && _param_com_rc_in_mode.get() != 3
&& _param_com_rc_in_mode.get() != 5 && _param_com_rc_in_mode.get() != 6) {
return;
}

10
src/modules/commander/commander_params.c
View File

@ -173,19 +173,19 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
* A value of 2 allows either RC Transmitter or Joystick input. The first valid input is used, will fallback to other sources if the input stream becomes invalid.
* A value of 3 allows either input from RC or joystick. The first available source is selected and used until reboot.
* A value of 4 ignores any stick input.
* A value of 5 allows either RC Transmitter or Joystick input. But RC has priority and whenever avaiable is immedietely used.
* A value of 6 allows either RC Transmitter or Joystick input. But Joystick has priority and whenever avaiable is immedietely used.
* A value of 5 allows either RC Transmitter or Joystick input, prioritizing by ascending source ID, giving an RC transmitter priority over MAVLink joysticks, and lower MAVLink instance numbers priority over higher ones.
* A value of 6 allows either RC Transmitter or Joystick input, prioritizing by descending source ID, giving MAVLink joysticks priority over an RC transmitter, and higher MAVLink instance numbers priority over lower ones.
*
* @group Commander
* @min 0
* @max 4
* @max 6
* @value 0 RC Transmitter only
* @value 1 Joystick only
* @value 2 RC and Joystick with fallback
* @value 3 RC or Joystick keep first
* @value 4 Stick input disabled
* @value 5 RC priority, Joystick fallback
* @value 6 Joystick priority, RC fallback
* @value 5 Ascending priority
* @value 6 Descending priority
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);

4
src/modules/commander/failsafe/failsafe.h
View File

@ -132,7 +132,9 @@ private:
JoystickOnly = 1, // Joystick only
RcAndJoystickWithFallback = 2, // RC And Joystick with fallback
RcOrJoystickKeepFirst = 3, // RC or Joystick keep first
StickInputDisabled = 4 // input disabled
StickInputDisabled = 4, // input disabled
AscendingPriority = 5, // ascending source priority
DescendingPriority = 6 // descending source priority
};
enum class command_after_high_wind_failsafe : int32_t {

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

@ -43,20 +43,16 @@ void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance)
{
if (input.valid) {
if (isRc(input.data_source)) { _timestamp_last_rc = input.timestamp_sample; }
if (isMavlink(input.data_source)) { _timestamp_last_mavlink = input.timestamp_sample; }
}
// 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.data_source);
const bool start_using_new_input = !_setpoint.valid;
const bool is_priority_mode = (_rc_in_mode == RcInMode::AscendingPriority
|| _rc_in_mode == RcInMode::DescendingPriority);
// 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)) {
if (isInputValid(input, now) && (update_existing_input || start_using_new_input || is_priority_mode)) {
_setpoint = input;
_setpoint.valid = true;
_setpoint.timestamp = now; // timestamp_sample is preserved
@ -78,32 +74,32 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
bool match = false;
switch (_rc_in_mode) { // COM_RC_IN_MODE
case 0: // RC Transmitter only
case RcInMode::RcTransmitterOnly:
match = isRc(input.data_source);
break;
case 1: // Joystick only
case RcInMode::JoystickOnly:
match = isMavlink(input.data_source);
break;
case 2: // RC and Joystick with fallback
case RcInMode::RcAndJoystickWithFallback:
match = true;
break;
case 3: // RC or Joystick keep first
case RcInMode::RcOrJoystickKeepFirst:
match = (input.data_source == _first_valid_source)
|| (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
break;
case 5: // RC priority, Joystick fallback
match = isRc(input.data_source) || (now > _timestamp_last_rc + _timeout);
case RcInMode::AscendingPriority:
match = !_setpoint.valid || (input.data_source <= _setpoint.data_source);
break;
case 6: // Joystick priority, RC fallback
match = isMavlink(input.data_source) || (now > _timestamp_last_mavlink + _timeout);
case RcInMode::DescendingPriority:
match = !_setpoint.valid || (input.data_source >= _setpoint.data_source);
break;
case 4: // Stick input disabled
case RcInMode::StickInputDisabled:
default:
break;
}

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

@ -39,7 +39,7 @@
class ManualControlSelector
{
public:
void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; }
void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = static_cast<RcInMode>(rc_in_mode); }
void setTimeout(uint64_t timeout) { _timeout = timeout; }
void updateValidityOfChosenInput(uint64_t now);
void updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance);
@ -51,11 +51,20 @@ private:
static bool isRc(uint8_t source);
static bool isMavlink(uint8_t source);
// COM_RC_IN_MODE parameter values
enum class RcInMode : int32_t {
RcTransmitterOnly = 0, // RC Transmitter only
JoystickOnly = 1, // Joystick only
RcAndJoystickWithFallback = 2, // RC And Joystick with fallback
RcOrJoystickKeepFirst = 3, // RC or Joystick keep first
StickInputDisabled = 4, // input disabled
AscendingPriority = 5, // ascending source priority
DescendingPriority = 6 // descending source priority
};
manual_control_setpoint_s _setpoint{};
uint64_t _timeout{0};
int32_t _rc_in_mode{0};
RcInMode _rc_in_mode{RcInMode::RcTransmitterOnly};
int _instance{-1};
uint8_t _first_valid_source{manual_control_setpoint_s::SOURCE_UNKNOWN};
uint64_t _timestamp_last_rc{0};
uint64_t _timestamp_last_mavlink{0};
};

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

@ -352,15 +352,15 @@ TEST(ManualControlSelector, RcOutdated)
EXPECT_EQ(selector.instance(), -1);
}
TEST(ManualControlSelector, RcMavlinkInputRcPriority)
TEST(ManualControlSelector, AscendingSourcePriority)
{
ManualControlSelector selector;
selector.setRcInMode(5); // Configure RC priority
selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid RC input gets used
// Valid RC input (ID 1) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_RC;
input.valid = true;
@ -373,7 +373,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
timestamp += 100_ms;
// Now provide input from MAVLink as well which should get ignored
// Now provide input from MAVLink (ID 2) as well which should get ignored (1 < 2)
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@ -384,7 +384,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
timestamp += 500_ms;
// Now we update MAVLink and let RC time out, so it should switch to RC
// Now we update MAVLink and let RC time out, so it should switch to MAVLink
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@ -393,7 +393,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
// If we get RC back immediately, it should use it with priority
// If we get RC back immediately, it should take over with priority (1 < 2)
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@ -401,17 +401,69 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
TEST(ManualControlSelector, MavlinkTwoInstanceAscendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure MAVLink priority
selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink input gets used
// Valid MAVLink 1 input (ID 3) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_1;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
timestamp += 100_ms;
// Now provide input from MAVLink 0 (ID 2), which has higher priority (2 < 3) and should take over
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
// If we get MAVLink 1 back immediately, it should be ignored since MAVLink 0 has higher priority (2 < 3)
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we update MAVLink 1 and let MAVLink 0 time out, so it should switch to MAVLink 1
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, DescendingSourcePriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure descending source ID priority (higher number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink input (ID 2) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
@ -424,7 +476,7 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
timestamp += 100_ms;
// Now provide input from RC as well which should get ignored
// Now provide input from RC (ID 1) as well which should get ignored (2 > 1)
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@ -444,7 +496,7 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 1);
// If we get MAVLink back immediately, it should use it with priority
// If we get MAVLink back immediately, it should take over with priority (2 > 1)
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@ -454,15 +506,15 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
TEST(ManualControlSelector, MavlinkTwoInstanceDescendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure MAVLink priority
selector.setRcInMode(6); // Configure descending source ID priority (higher number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink 0 input gets used
// Valid MAVLink 0 input (ID 2) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
@ -475,18 +527,7 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
timestamp += 100_ms;
// Now provide input from MAVLink 1 as well which should get ignored
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we update MAVLink 1 and let MAVLink 0 time out, so it should switch to MAVLink 1
// Now provide input from MAVLink 1 (ID 3), which has higher priority (3 > 2) and should take over
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@ -495,7 +536,7 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
// If we get MAVLink 0 back immediately, it should not switch since MAVLink 1 has the same priority
// If we get MAVLink 0 back immediately, it should be ignored since MAVLink 1 has higher priority (3 > 2)
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@ -503,4 +544,15 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
timestamp += 500_ms;
// Now we update MAVLink 0 and let MAVLink 1 time out, so it should switch to MAVLink 0
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
}