Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

FlightTaskDescend: Enable nudging by sticks when it's enabled

Browse Source 
and stick input is available.
This commit is contained in:
Matthias Grob
2023-02-24 11:55:39 +01:00
parent 728570828f
commit 0770478dc7
3 changed files with 36 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt
+2 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-2023 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
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskDescend
FlightTaskDescend.cpp
)
target_link_libraries(FlightTaskDescend PUBLIC FlightTask)
target_link_libraries(FlightTaskDescend PUBLIC FlightTask FlightTaskUtility)
target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp
+23 -12
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2023 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
@@ -36,16 +36,6 @@
#include "FlightTaskDescend.hpp"
bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
// stay level to minimize horizontal drift
_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
// keep heading
_yaw_setpoint = _yaw;
return ret;
}
bool FlightTaskDescend::update()
{
bool ret = FlightTask::update();
@@ -58,7 +48,28 @@ bool FlightTaskDescend::update()
} else {
// descend with constant acceleration (crash landing)
_velocity_setpoint(2) = NAN;
_acceleration_setpoint(2) = .3f;
_acceleration_setpoint(2) = .15f;
}
// Nudging
if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
_is_yaw_good_for_control, _deltatime);
_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
_yaw_setpoint);
// Stick full up -1 -> stop, stick full down 1 -> double the value
_velocity_setpoint(2) *= (1 - _sticks.getThrottleZeroCenteredExpo());
_acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f;
} else {
_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); // stay level to minimize horizontal drift
_yawspeed_setpoint = NAN;
// keep heading
if (!PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint = _yaw;
}
}
return ret;
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp
+11 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2023 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
@@ -38,6 +38,9 @@
#pragma once
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include "StickTiltXY.hpp"
#include "StickYaw.hpp"
class FlightTaskDescend : public FlightTask
{
@@ -46,11 +49,15 @@ public:
virtual ~FlightTaskDescend() = default;
bool update() override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
private:
Sticks _sticks{this};
StickTiltXY _stick_tilt_xy{this};
StickYaw _stick_yaw{this};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, ///< thrust at hover equilibrium
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed ///< velocity for controlled descend
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover ///< thrust at hover equilibrium
)
};