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

rc_update: add unit tests for mode slot 

To verify RC mode switch and mode button functionality works as expected.
This commit is contained in:
Matthias Grob 2022-05-03 19:18:12 +02:00
parent 3fe4c6e3f5
commit 4bd2d4cf35
5 changed files with 145 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/commander/failure_detector/FailureDetector.hpp
View File

@ -43,11 +43,11 @@
#pragma once
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <px4_platform_common/module_params.h>
#include <hysteresis/hysteresis.h>
// subscriptions
#include <uORB/Subscription.hpp>

3
src/modules/rc_update/CMakeLists.txt
View File

@ -38,6 +38,9 @@ px4_add_module(
rc_update.cpp
rc_update.h
DEPENDS
hysteresis
mathlib
px4_work_queue
)
px4_add_functional_gtest(SRC RCUpdateTest.cpp LINKLIBS modules__rc_update)

133
src/modules/rc_update/RCUpdateTest.cpp Normal file
View File

@ -0,0 +1,133 @@
/****************************************************************************
*
* Copyright (C) 2022 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.
*
****************************************************************************/
#define MODULE_NAME "rc_update"
#include <gtest/gtest.h>
#include "rc_update.h"
using namespace rc_update;
TEST(RCUpdateTest, ModeSlotUnassigned)
{
RCUpdate rc_update;
// GIVEN: Default configuration with no assigned mode switch
EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 0);
// WHEN: we update the switches two times to pass the simple outlier protection
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(0);
// THEN: we receive no mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();
EXPECT_EQ(manual_control_switches_sub.get().mode_slot, 0); // manual_control_switches_s::MODE_SLOT_NONE
}
void checkModeSlotSwitch(float channel_value, uint8_t expected_slot)
{
RCUpdate rc_update;
// GIVEN: First channel is configured as mode switch
rc_update._param_rc_map_fltmode.set(1);
EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 1);
// GIVEN: First channel has some value
rc_update._rc.channels[0] = channel_value;
// WHEN: we update the switches two times to pass the simple outlier protection
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(0);
// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();
EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
}
TEST(RCUpdateTest, ModeSlotSwitchAllValues)
{
checkModeSlotSwitch(-1.f, 1); // manual_control_switches_s::MODE_SLOT_1
checkModeSlotSwitch(-.5f, 2); // manual_control_switches_s::MODE_SLOT_2
checkModeSlotSwitch(-.1f, 3); // manual_control_switches_s::MODE_SLOT_3
checkModeSlotSwitch(0.f, 4); // manual_control_switches_s::MODE_SLOT_4
checkModeSlotSwitch(.5f, 5); // manual_control_switches_s::MODE_SLOT_5
checkModeSlotSwitch(1.f, 6); // manual_control_switches_s::MODE_SLOT_6
}
void checkModeSlotButton(uint8_t button_configuration, uint8_t channel, float channel_value, uint8_t expected_slot)
{
RCUpdate rc_update;
// GIVEN: Buttons are configured
rc_update._param_rc_map_fltm_btn.set(button_configuration);
EXPECT_EQ(rc_update._param_rc_map_fltm_btn.get(), button_configuration);
// GIVEN: buttons are mapped
rc_update.update_rc_functions();
// GIVEN: First channel has some value
rc_update._rc.channels[channel - 1] = channel_value;
// WHEN: we update the switches 4 times:
// - initiate the button press
// - keep the same button pressed
// - hold the button for 50ms
// - pass the simple outlier protection
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(51_ms);
rc_update.UpdateManualSwitches(51_ms);
// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();
EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
}
TEST(RCUpdateTest, ModeSlotButtonAllValues)
{
checkModeSlotButton(1, 1, -1.f, 0); // button not pressed -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(1, 1, 0.f, 0); // button not pressed over threshold -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(1, 1, 1.f, 1); // button 1 pressed -> manual_control_switches_s::MODE_SLOT_1
checkModeSlotButton(1, 2, 1.f, 0); // button 2 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(3, 2, 1.f, 2); // button 2 pressed -> manual_control_switches_s::MODE_SLOT_2
checkModeSlotButton(3, 3, 1.f, 0); // button 3 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(7, 3, 1.f, 3); // button 3 pressed -> manual_control_switches_s::MODE_SLOT_3
checkModeSlotButton(7, 4, 1.f, 0); // button 4 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(15, 4, 1.f, 4); // button 4 pressed -> manual_control_switches_s::MODE_SLOT_4
checkModeSlotButton(15, 5, 1.f, 0); // button 5 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(31, 5, 1.f, 5); // button 5 pressed -> manual_control_switches_s::MODE_SLOT_5
checkModeSlotButton(31, 6, 1.f, 0); // button 6 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(63, 6, 1.f, 6); // button 6 pressed -> manual_control_switches_s::MODE_SLOT_6
}

8
src/modules/rc_update/rc_update.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 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
@ -320,7 +320,7 @@ RCUpdate::map_flight_modes_buttons()
}
// If the functionality is disabled we don't need to map channels
const int flightmode_buttons = _param_rc_map_flightmode_buttons.get();
const int flightmode_buttons = _param_rc_map_fltm_btn.get();
if (flightmode_buttons == 0) {
return;
@ -605,7 +605,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
switches.mode_slot = num_slots;
}
} else if (_param_rc_map_flightmode_buttons.get() > 0) {
} else if (_param_rc_map_fltm_btn.get() > 0) {
switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE;
bool is_consistent_button_press = false;
@ -629,7 +629,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
}
}
_button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, hrt_absolute_time());
_button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, timestamp_sample);
if (_button_pressed_hysteresis.get_state()) {
switches.mode_slot = _potential_button_press_slot;

8
src/modules/rc_update/rc_update.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 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
@ -46,6 +46,7 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
@ -59,7 +60,6 @@
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/parameter_update.h>
#include <hysteresis/hysteresis.h>
using namespace time_literals;
@ -90,8 +90,6 @@ public:
int print_status() override;
private:
static constexpr uint64_t VALID_DATA_MIN_INTERVAL_US{1_s / 3}; // assume valid RC input is at least 3 Hz
void Run() override;
@ -219,7 +217,7 @@ private:
(ParamInt<px4::params::RC_MAP_ARM_SW>) _param_rc_map_arm_sw,
(ParamInt<px4::params::RC_MAP_TRANS_SW>) _param_rc_map_trans_sw,
(ParamInt<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw,
(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons,
(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_fltm_btn,
(ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1,
(ParamInt<px4::params::RC_MAP_AUX2>) _param_rc_map_aux2,