fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 12:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

new manual_control_switches msg (split out of manual_control_setpoint) (#16270)

Browse Source 
- split out switches from manual_control_setpoint into new message manual_control_switches
 - manual_control_switches published at minimal rate (~ 1 Hz) or immediately on change
 - simple switch debounce in rc_update (2 consecutive identical decodes required)
 - manual_control_switches logged at full rate rather than sampled at (5-10% of messages logged)
 - manual_control_setpoint publish at minimal rate unless changing
 - commander handle landing gear switch for manual modes
 - processing of mode_slot and mode_switch is now split so we only do one or the other (not both)
     - a future step will be to finally drop mode_switch and accompanying switches entirely

Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2020-12-11 12:11:35 -05:00
committed by GitHub
co-authored by Matthias Grob
parent 25ef76b3b8
commit ef6209ba03
25 changed files with 670 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+2 -2
View File
@@ -42,7 +42,7 @@ using namespace matrix;
FlightTaskAutoMapper::FlightTaskAutoMapper() :
_sticks(this)
{};
{}
bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
@@ -187,7 +187,7 @@ float FlightTaskAutoMapper::_getLandSpeed()
// user input assisted land speed
if (_param_mpc_land_rc_help.get()
&& (_dist_to_ground < _param_mpc_land_alt1.get())
&& _sticks.checkAndSetStickInputs(_time_stamp_current)) {
&& _sticks.checkAndSetStickInputs()) {
// stick full up -1 -> stop, stick full down 1 -> double the speed
land_speed *= (1 + _sticks.getPositionExpo()(2));
}
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+2 -3
View File
@@ -44,14 +44,13 @@ using namespace matrix;
FlightTaskManualAltitude::FlightTaskManualAltitude() :
_sticks(this)
{};
{}
bool FlightTaskManualAltitude::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
_sticks.checkAndSetStickInputs(_time_stamp_current);
_sticks.setGearAccordingToSwitch(_gear);
_sticks.checkAndSetStickInputs();
if (_sticks_data_required) {
ret = ret && _sticks.isAvailable();
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+3
View File
@@ -42,6 +42,7 @@
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
#include <uORB/Subscription.hpp>
class FlightTaskManualAltitude : public FlightTask
{
@@ -129,6 +130,8 @@ private:
*/
void _respectGroundSlowdown();
void setGearAccordingToSwitch();
float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
float _max_speed_up = 10.0f;
src/lib/flight_tasks/tasks/Utility/Sticks.cpp
+21 -42
View File
@@ -44,36 +44,40 @@ Sticks::Sticks(ModuleParams *parent) :
ModuleParams(parent)
{};
bool Sticks::checkAndSetStickInputs(hrt_abstime now)
bool Sticks::checkAndSetStickInputs()
{
_sub_manual_control_setpoint.update();
hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
// Sticks are rescaled linearly and exponentially to [-1,1]
if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// Linear scale
_positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1]
_positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1]
_positions(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
_positions(3) = _sub_manual_control_setpoint.get().r; // yaw [-1,1]
_positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
_positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
_positions(2) = -(manual_control_setpoint.z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
// Exponential scale
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
// valid stick inputs are required
const bool valid_sticks = PX4_ISFINITE(_positions(0))
&& PX4_ISFINITE(_positions(1))
&& PX4_ISFINITE(_positions(2))
&& PX4_ISFINITE(_positions(3));
const bool valid_sticks = PX4_ISFINITE(_positions(0))
&& PX4_ISFINITE(_positions(1))
&& PX4_ISFINITE(_positions(2))
&& PX4_ISFINITE(_positions(3));
_input_available = valid_sticks;
} else {
_input_available = false;
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
if (vehicle_status.rc_signal_lost) {
_input_available = false;
}
}
}
if (!_input_available) {
@@ -85,31 +89,6 @@ bool Sticks::checkAndSetStickInputs(hrt_abstime now)
return _input_available;
}
void Sticks::setGearAccordingToSwitch(landing_gear_s &gear)
{
// Only switch the landing gear up if the user switched from gear down to gear up.
// If the user had the switch in the gear up position and took off ignore it
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
if (!isAvailable()) {
gear.landing_gear = landing_gear_s::GEAR_KEEP;
} else {
const int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
if (_gear_switch_old != gear_switch) {
if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
gear.landing_gear = landing_gear_s::GEAR_UP;
}
}
_gear_switch_old = gear_switch;
}
}
void Sticks::limitStickUnitLengthXY(Vector2f &v)
{
const float vl = v.length();
src/lib/flight_tasks/tasks/Utility/Sticks.hpp
+6 -8
View File
@@ -44,8 +44,8 @@
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
class Sticks : public ModuleParams
{
@@ -53,8 +53,7 @@ public:
Sticks(ModuleParams *parent);
~Sticks() = default;
bool checkAndSetStickInputs(hrt_abstime now);
void setGearAccordingToSwitch(landing_gear_s &gear);
bool checkAndSetStickInputs();
bool isAvailable() { return _input_available; };
const matrix::Vector<float, 4> &getPosition() { return _positions; };
const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
@@ -75,18 +74,17 @@ public:
static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint);
private:
bool _input_available = false;
bool _input_available{false};
matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs
matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state
uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
(ParamFloat<px4::params::MPC_XY_MAN_EXPO>) _param_mpc_xy_man_expo,
(ParamFloat<px4::params::MPC_Z_MAN_EXPO>) _param_mpc_z_man_expo,
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo
)
};