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

FlightTasks: move stick handling into library

Browse Source
This commit is contained in:
Matthias Grob 2020-07-14 13:22:47 +02:00 committed by Julian Kent
parent 49a543e1b2
commit 9daff24e79
11 changed files with 202 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/flight_tasks/tasks/Manual/CMakeLists.txt
View File

@ -35,5 +35,5 @@ px4_add_library(FlightTaskManual
FlightTaskManual.cpp
)
target_link_libraries(FlightTaskManual PUBLIC FlightTask)
target_link_libraries(FlightTaskManual PUBLIC FlightTask FlightTaskUtility)
target_include_directories(FlightTaskManual PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

66
src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp
View File

@ -42,13 +42,15 @@
using namespace matrix;
using namespace time_literals;
FlightTaskManual::FlightTaskManual() :
_sticks(this)
{};
bool FlightTaskManual::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
_sub_manual_control_setpoint.update();
const bool sticks_available = _evaluateSticks();
const bool sticks_available = _sticks.evaluateSticks(_time_stamp_current, _gear);
if (_sticks_data_required) {
ret = ret && sticks_available;
@ -56,61 +58,3 @@ bool FlightTaskManual::updateInitialize()
return ret;
}
bool FlightTaskManual::_evaluateSticks()
{
hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
/* Sticks are rescaled linearly and exponentially to [-1,1] */
if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
/* Linear scale */
_sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */
_sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */
_sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
_sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */
/* Exponential scale */
_sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
// 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.
int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
if (_gear_switch_old != gear_switch) {
_applyGearSwitch(gear_switch);
}
_gear_switch_old = gear_switch;
// valid stick inputs are required
const bool valid_sticks = PX4_ISFINITE(_sticks(0))
&& PX4_ISFINITE(_sticks(1))
&& PX4_ISFINITE(_sticks(2))
&& PX4_ISFINITE(_sticks(3));
return valid_sticks;
} else {
/* Timeout: set all sticks to zero */
_sticks.zero();
_sticks_expo.zero();
_gear.landing_gear = landing_gear_s::GEAR_KEEP;
return false;
}
}
void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
{
if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
}

32
src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp
View File

@ -41,42 +41,16 @@
#pragma once
#include "FlightTask.hpp"
#include <uORB/topics/manual_control_setpoint.h>
#include "Sticks.hpp"
class FlightTaskManual : public FlightTask
{
public:
FlightTaskManual() = default;
FlightTaskManual();
virtual ~FlightTaskManual() = default;
bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); };
bool updateInitialize() override;
protected:
Sticks _sticks;
bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */
matrix::Vector<float, 4> _sticks; /**< unmodified manual stick inputs */
matrix::Vector<float, 4> _sticks_expo; /**< modified manual sticks using expo function*/
int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/
float stickDeadzone() const { return _param_mpc_hold_dz.get(); }
private:
bool _evaluateSticks(); /**< checks and sets stick inputs */
void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */
(ParamFloat<px4::params::MPC_XY_MAN_EXPO>)
_param_mpc_xy_man_expo, /**< ratio of exponential curve for stick input in xy direction */
(ParamFloat<px4::params::MPC_Z_MAN_EXPO>)
_param_mpc_z_man_expo, /**< ratio of exponential curve for stick input in z direction */
(ParamFloat<px4::params::MPC_YAW_EXPO>)
_param_mpc_yaw_expo, /**< ratio of exponential curve for stick input in yaw for modes except acro */
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t /**< time at which commander considers RC lost */
)
};

16
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@ -88,12 +88,12 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
void FlightTaskManualAltitude::_scaleSticks()
{
// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
// Use sticks input with deadzone and exponential curve for vertical velocity
const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
}
float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@ -110,7 +110,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
// If not locked, altitude setpoint is set to NAN.
// Check if user wants to break
const bool apply_brake = fabsf(_sticks_expo(2)) <= FLT_EPSILON;
const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;
// Check if vehicle has stopped
const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
@ -122,7 +122,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
float spd_xy = Vector2f(_velocity).length();
// Use presence of horizontal stick inputs as a transition criteria
float stick_xy = Vector2f(&_sticks_expo(0)).length();
float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length();
bool stick_input = stick_xy > 0.001f;
if (_terrain_hold) {
@ -340,7 +340,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
// setpoint along z-direction, which is computed in PositionControl.cpp.
Vector2f sp(&_sticks(0));
Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0));
_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
_man_input_filter.update(sp);
@ -359,8 +359,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
bool FlightTaskManualAltitude::_checkTakeoff()
{
// stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
return _sticks(2) < -0.3f;
// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])
return _sticks.getPosition()(2) < -0.3f;
}
bool FlightTaskManualAltitude::update()

2
src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp
View File

@ -40,7 +40,7 @@
using namespace matrix;
FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
_smoothing(this, _velocity(2), _sticks(2))
_smoothing(this, _velocity(2), _sticks.getPosition()(2))
{}
void FlightTaskManualAltitudeSmooth::_updateSetpoints()

2
src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
View File

@ -82,7 +82,7 @@ void FlightTaskManualPosition::_scaleSticks()
FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/
Vector2f stick_xy = _sticks_expo.slice<2, 1>(0, 0);
Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0);
const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);

2
src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp
View File

@ -41,7 +41,7 @@ using namespace matrix;
FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
_smoothingXY(this, Vector2f(_velocity)),
_smoothingZ(this, _velocity(2), _sticks(2))
_smoothingZ(this, _velocity(2), _sticks.getPosition()(2))
{}
void FlightTaskManualPositionSmooth::_updateSetpoints()

6
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -173,8 +173,8 @@ bool FlightTaskOrbit::update()
bool ret = FlightTaskManualAltitudeSmooth::update();
// stick input adjusts parameters within a fixed time frame
const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f);
const float v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f);
const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
setRadius(r);
setVelocity(v);
@ -257,7 +257,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position)
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
_yaw_setpoint = NAN;
_yawspeed_setpoint = _sticks_expo(3);
_yawspeed_setpoint = _sticks.getPositionExpo()(3);
break;
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:

1
src/lib/flight_tasks/tasks/Utility/CMakeLists.txt
View File

@ -38,6 +38,7 @@ px4_add_library(FlightTaskUtility
VelocitySmoothing.cpp
ManualVelocitySmoothingXY.cpp
ManualVelocitySmoothingZ.cpp
Sticks.cpp
)
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier)

104
src/lib/flight_tasks/tasks/Utility/Sticks.cpp Normal file
View File

@ -0,0 +1,104 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
/**
* @file Sticks.cpp
*/
#include "Sticks.hpp"
using namespace time_literals;
Sticks::Sticks(ModuleParams *parent) :
ModuleParams(parent)
{};
bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear)
{
_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) {
// 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]
// 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());
// 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.
int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
if (_gear_switch_old != gear_switch) {
applyGearSwitch(gear_switch, gear);
}
_gear_switch_old = gear_switch;
// 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));
return valid_sticks;
} else {
// Timeout: set all sticks to zero
_positions.zero();
_positions_expo.zero();
gear.landing_gear = landing_gear_s::GEAR_KEEP;
return false;
}
}
void Sticks::applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear)
{
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;
}
}

74
src/lib/flight_tasks/tasks/Utility/Sticks.hpp Normal file
View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
/**
* @file Sticks.hpp
*
* Library abstracting stick input from manual_control_setpoint
*
* @author Matthias Grob <maetugr@gmail.com>
*/
#pragma once
#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>
class Sticks : public ModuleParams
{
public:
Sticks(ModuleParams *parent);
~Sticks() = default;
bool evaluateSticks(hrt_abstime now, landing_gear_s &gear); ///< checks and sets stick inputs
void applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear); ///< Sets gears according to switch
const matrix::Vector<float, 4> &getPosition() { return _positions; };
const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
private:
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)};
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
)
};