mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 18:03:33 +08:00
FlightTaskManual/Alt/Position: update comments
This commit is contained in:
Dennis Mannhart
committed by
Beat Küng
Beat Küng
parent
55b6f19367
commit
720271332d
@@ -30,14 +30,11 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightTaskManual.hpp
|
||||
* @file FlightTaskManual.cpp
|
||||
*
|
||||
* Flight task for the normal, legacy, manual position controlled flight
|
||||
* where stick inputs map basically to the velocity setpoint
|
||||
* Linear and exponential map from stick inputs to range -1 and 1.
|
||||
*
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#include "FlightTaskManual.hpp"
|
||||
@@ -126,13 +123,16 @@ void FlightTaskManual::_updateYaw()
|
||||
|
||||
bool FlightTaskManual::_evaluateSticks()
|
||||
{
|
||||
/* Sticks are rescaled linearly and exponentially from [0,1] to [-1,1] */
|
||||
if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) {
|
||||
/* get data and scale correctly */
|
||||
|
||||
/* 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), _xy_vel_man_expo.get(), _hold_dz.get());
|
||||
_sticks_expo(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get());
|
||||
_sticks_expo(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get());
|
||||
@@ -140,7 +140,8 @@ bool FlightTaskManual::_evaluateSticks()
|
||||
return true;
|
||||
|
||||
} else {
|
||||
_sticks.zero(); /* default is all zero */
|
||||
/* Timeout: set all sticks to zero */
|
||||
_sticks.zero();
|
||||
_sticks_expo.zero();
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -34,10 +34,8 @@
|
||||
/**
|
||||
* @file FlightTaskManual.hpp
|
||||
*
|
||||
* Flight task for the normal, manual position controlled flight
|
||||
* where stick inputs map basically to the velocity setpoint
|
||||
* Linear and exponential map from stick inputs to range -1 and 1.
|
||||
*
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
@@ -65,17 +63,17 @@ public:
|
||||
|
||||
protected:
|
||||
|
||||
bool _sticks_data_required = true; /**< let sibling task define if it depends on stick data */
|
||||
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::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
|
||||
control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */
|
||||
control::BlockParamFloat _hold_dz; /**< 0-deadzone around the center for the sticks */
|
||||
|
||||
private:
|
||||
|
||||
uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
|
||||
|
||||
control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */
|
||||
control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */
|
||||
control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction */
|
||||
control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in z direction */
|
||||
|
||||
|
||||
bool _evaluateSticks(); /**< checks and sets stick inputs */
|
||||
|
||||
@@ -32,10 +32,9 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightTaskManual.hpp
|
||||
*
|
||||
* Flight-task for manual controlled altitude.
|
||||
* @file FlightManualAltitude.cpp
|
||||
*
|
||||
* Flight task for manual controlled altitude.
|
||||
*/
|
||||
|
||||
#include "FlightTaskManualAltitude.hpp"
|
||||
@@ -67,23 +66,25 @@ bool FlightTaskManualAltitude::activate()
|
||||
|
||||
void FlightTaskManualAltitude::scaleSticks()
|
||||
{
|
||||
|
||||
/* Map stick to velocity */
|
||||
/* Map stick to velocity. */
|
||||
const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
|
||||
_vel_sp_z = vel_max_z * _sticks_expo(2);
|
||||
_yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
|
||||
|
||||
}
|
||||
|
||||
void FlightTaskManualAltitude::updateHeadingSetpoints()
|
||||
{
|
||||
/* Yaw-lock depends on stick input. If locked,
|
||||
* yawspeed_sp is set to NAN. Otherwise yaw_sp is set
|
||||
* to NAN.*/
|
||||
|
||||
if (fabsf(_sticks(3)) < _hold_dz.get()) {
|
||||
/* Want to hold yaw */
|
||||
/* Hold yaw */
|
||||
_yaw_rate_sp = NAN;
|
||||
_yaw_sp = _yaw_sp_predicted;
|
||||
|
||||
} else {
|
||||
/* Want to change yaw based on yaw-rate */
|
||||
/* Change yaw through yaw-rate inputs.*/
|
||||
_yaw_sp = NAN;
|
||||
_yaw_sp_predicted = _wrap_pi(_yaw_sp_predicted + _yaw_rate_sp * _deltatime);
|
||||
}
|
||||
@@ -91,28 +92,33 @@ void FlightTaskManualAltitude::updateHeadingSetpoints()
|
||||
|
||||
void FlightTaskManualAltitude::updateZsetpoints()
|
||||
{
|
||||
/* Depending on stick inputs, position is locked or
|
||||
* velocity setpoint is used. If locked, velocity setpoint
|
||||
* is set to NAN. Otherwise position setpoints is set to NAN.
|
||||
*/
|
||||
if (fabsf(_sticks_expo(2)) < FLT_EPSILON) {
|
||||
|
||||
/* Want to hold altitude */
|
||||
/* Hold position */
|
||||
|
||||
if (_lock_time <= _lock_time_max) {
|
||||
/* Don't lock: time has not been reached */
|
||||
/* Don't lock: time has not been reached yet. */
|
||||
_vel_sp_z = 0.0f;
|
||||
_pos_sp_z = NAN;
|
||||
_pos_sp_z_lock = _position(2);
|
||||
_lock_time += _deltatime;
|
||||
|
||||
} else {
|
||||
/* Lock position in z. */
|
||||
_vel_sp_z = NAN;
|
||||
_pos_sp_z = _pos_sp_z_lock;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* Want to change altitude through velocity */
|
||||
/* Change z-direction based on velocity input. Hence, set
|
||||
* position setpoin in z to NAN since not used. */
|
||||
_pos_sp_z = NAN;
|
||||
|
||||
/* Maximum brake acceleration depends
|
||||
* on direction. We take half of max acceleration
|
||||
* on direction (up or down). We take half of max acceleration
|
||||
* because it is better to lock late than early to prevent
|
||||
* up and down movement.
|
||||
*/
|
||||
@@ -137,10 +143,8 @@ void FlightTaskManualAltitude::updateSetpoints()
|
||||
|
||||
bool FlightTaskManualAltitude::update()
|
||||
{
|
||||
scaleSticks();
|
||||
updateSetpoints();
|
||||
|
||||
|
||||
scaleSticks(); // scales sticks to yawspeed and velocity
|
||||
updateSetpoints(); // applies yaw and position lock if required
|
||||
_setPositionSetpoint(Vector3f(NAN, NAN, _pos_sp_z));
|
||||
_setVelocitySetpoint(Vector3f(NAN, NAN, _vel_sp_z));
|
||||
_setYawSetpoint(_yaw_sp);
|
||||
|
||||
@@ -32,10 +32,9 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightTaskOrbit.hpp
|
||||
* @file FlightManualAltitude.hpp
|
||||
*
|
||||
* Flight task for manual controlled altitude.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
@@ -54,26 +53,26 @@ public:
|
||||
bool update() override;
|
||||
|
||||
protected:
|
||||
float _yaw_rate_sp{}; /** scaled yaw rate directly from stick. NAN if yaw is locked */
|
||||
float _yaw_sp{}; /** yaw setpoint once locked. otherwise NAN */
|
||||
float _yaw_sp_predicted{}; /** _yaw_sp during lock; predicted yaw through _yaw_rate_sp demand */
|
||||
float _vel_sp_z{}; /**< scaled velocity directly from stick. During altitude lock is equal to NAN */
|
||||
float _pos_sp_z{}; /**< altitude setpoint in z during lock. Otherwise NAN. */
|
||||
float _pos_sp_z_lock{}; /**< altitude which used when lock is engaged */
|
||||
float _yaw_rate_sp{}; /**< Scaled yaw rate from stick. NAN if yaw is locked. */
|
||||
float _yaw_sp{}; /**< Yaw setpoint once locked. Otherwise NAN. */
|
||||
float _yaw_sp_predicted{}; /** Equal to_yaw_sp during lock. Predicted yaw_sp during non-lock.*/
|
||||
float _vel_sp_z{}; /**< Scaled velocity from stick. During altitude lock it is equal to NAN. */
|
||||
float _pos_sp_z{}; /**< Setpoint in z during lock. Otherwise NAN. */
|
||||
float _pos_sp_z_lock{}; /**< Setpoint in z when lock is engaged. */
|
||||
|
||||
control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
|
||||
control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */
|
||||
control::BlockParamFloat _yaw_rate_scaling; /**< scaling factor from stick to yaw rate */
|
||||
control::BlockParamFloat _acc_max_up; /**< maximum acceleration upward */
|
||||
control::BlockParamFloat _acc_max_down; /**< maximum acceleration downward */
|
||||
control::BlockParamFloat _vel_max_down; /**< Maximum speed allowed to go up. */
|
||||
control::BlockParamFloat _vel_max_up; /**< Maximum speed allowed to go down. */
|
||||
control::BlockParamFloat _yaw_rate_scaling; /**< Scaling factor from stick to yaw rate. */
|
||||
control::BlockParamFloat _acc_max_up; /**< Maximum acceleration upward. */
|
||||
control::BlockParamFloat _acc_max_down; /**< Maximum acceleration downward. */
|
||||
|
||||
virtual void updateSetpoints(); /**< updates all setpoints */
|
||||
virtual void scaleSticks(); /**< scales sticks to velocity */
|
||||
virtual void updateSetpoints(); /**< Updates all setpoints. */
|
||||
virtual void scaleSticks(); /**< Scales sticks to velocity. */
|
||||
|
||||
private:
|
||||
void updateHeadingSetpoints(); /**< sets yaw or yaw speed */
|
||||
void updateZsetpoints(); /**< sets position or velocity setpoint */
|
||||
void updateHeadingSetpoints(); /**< Sets yaw or yaw speed. */
|
||||
void updateZsetpoints(); /**< Sets position or velocity setpoints. */
|
||||
|
||||
float _lock_time_max{0.0f}; /**< defines time when altitude lock occurs */
|
||||
float _lock_time{0.0f}; /**< time after stick are at zero position */
|
||||
float _lock_time_max{0.0f}; /**< Defines time when altitude lock occurs. */
|
||||
float _lock_time{0.0f}; /**< Time passed when stick is at center position and before lock occurs.*/
|
||||
};
|
||||
|
||||
@@ -32,9 +32,9 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightTaskManual.hpp
|
||||
* @file FlightTaskManualPosition.cpp
|
||||
*
|
||||
* Flight task for manual controlled position.
|
||||
* Flight task for manual position controlled mode.
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -62,50 +62,54 @@ bool FlightTaskManualPosition::activate()
|
||||
|
||||
void FlightTaskManualPosition::scaleSticks()
|
||||
{
|
||||
/* scale all sticks including altitude and yaw */
|
||||
/* Get velocity in z and yawspeed. */
|
||||
FlightTaskManualAltitude::scaleSticks();
|
||||
|
||||
/* Constrain length of stick inputs to 1. */
|
||||
matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));
|
||||
|
||||
/* constrain length of stick inputs to 1 */
|
||||
float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
|
||||
stick_xy = stick_xy.normalized() * mag;
|
||||
|
||||
/* scale to velocity */
|
||||
/* Scale to velocity.*/
|
||||
_vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
|
||||
|
||||
/* rotate setpoint into local frame */
|
||||
/* Rotate setpoint into local frame. */
|
||||
matrix::Vector3f vel_sp{_vel_sp_xy(0), _vel_sp_xy(1), 0.0f};
|
||||
vel_sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw_sp_predicted)) * vel_sp);
|
||||
_vel_sp_xy = matrix::Vector2f(vel_sp(0), vel_sp(1));
|
||||
|
||||
}
|
||||
|
||||
void FlightTaskManualPosition::updateXYsetpoints()
|
||||
{
|
||||
/* If position lock is active, velocity setpoint is set to NAN. Otherwise
|
||||
* position setpoint is set to NAN.
|
||||
*/
|
||||
|
||||
matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));
|
||||
|
||||
if (stick_xy.length() < FLT_EPSILON) {
|
||||
|
||||
/* Want to hold position */
|
||||
/* Hold position */
|
||||
|
||||
if (_lock_time <= _lock_time_max) {
|
||||
/* Don't lock: time has not been reached */
|
||||
/* Don't lock: time has not been reached. */
|
||||
_vel_sp_xy.zero();
|
||||
_pos_sp_xy = matrix::Vector2f(NAN, NAN);
|
||||
_pos_sp_xy_lock = matrix::Vector2f(&(_position(0)));
|
||||
_lock_time += _deltatime;
|
||||
|
||||
} else {
|
||||
/* want to hold position */
|
||||
/*Lock position. */
|
||||
_vel_sp_xy = matrix::Vector2f(NAN, NAN);
|
||||
_pos_sp_xy = matrix::Vector2f(&(_pos_sp_xy_lock(0)));
|
||||
}
|
||||
|
||||
} else {
|
||||
/* Want to change position */
|
||||
/* Move based on velocity. Position setpoint
|
||||
* not used and therefore set to NAN. */
|
||||
_pos_sp_xy = matrix::Vector2f(NAN, NAN);
|
||||
|
||||
/* Time to brake depends on
|
||||
/* Always update time to brake. It depends on
|
||||
* maximum acceleration.
|
||||
*/
|
||||
if (PX4_ISFINITE(_acc_xy_max.get())) {
|
||||
@@ -124,20 +128,15 @@ void FlightTaskManualPosition::updateXYsetpoints()
|
||||
|
||||
void FlightTaskManualPosition::updateSetpoints()
|
||||
{
|
||||
/* scale all sticks */
|
||||
scaleSticks();
|
||||
|
||||
/* update all setpoints */
|
||||
/* Apply position lock if required. */
|
||||
FlightTaskManualAltitude::updateSetpoints();
|
||||
updateXYsetpoints();
|
||||
|
||||
}
|
||||
|
||||
bool FlightTaskManualPosition::update()
|
||||
{
|
||||
scaleSticks();
|
||||
updateSetpoints();
|
||||
|
||||
_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
|
||||
_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
|
||||
_setYawSetpoint(_yaw_sp);
|
||||
|
||||
@@ -32,9 +32,9 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightTaskOrbit.hpp
|
||||
* @file FlightTaskManualPosition.hpp
|
||||
*
|
||||
* Flight task for manual controlled position.
|
||||
* Flight task for manual position controlled mode.
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -54,18 +54,18 @@ public:
|
||||
bool update() override;
|
||||
|
||||
protected:
|
||||
matrix::Vector2f _vel_sp_xy{}; /* Scaled velocity setpoint from stick. NAN during position lock. */
|
||||
matrix::Vector2f _pos_sp_xy{}; /* Position setpoint during lock. Otherwise NAN.*/
|
||||
matrix::Vector2f _pos_sp_xy_lock{}; /* Position setpoint during lock */
|
||||
matrix::Vector2f _vel_sp_xy{}; /**< Scaled velocity setpoint from stick. NAN during position lock. */
|
||||
matrix::Vector2f _pos_sp_xy{}; /**< Position setpoint during lock. Otherwise NAN.*/
|
||||
matrix::Vector2f _pos_sp_xy_lock{}; /**< Position setpoint during lock */
|
||||
|
||||
control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
|
||||
control::BlockParamFloat _acc_xy_max;
|
||||
control::BlockParamFloat _vel_xy_manual_max; /**< Maximum speed allowed horizontally, */
|
||||
control::BlockParamFloat _acc_xy_max;/**< Maximum acceleration horizontally. Only used to compute lock time. */
|
||||
|
||||
void updateSetpoints() override;
|
||||
void scaleSticks() override;
|
||||
|
||||
private:
|
||||
void updateXYsetpoints();
|
||||
float _lock_time_max{0.0f}; /**< defines time when position lock occurs */
|
||||
float _lock_time{0.0f}; /**< time after stick are at zero position */
|
||||
float _lock_time_max{0.0f}; /**< Defines time when position lock occurs. */
|
||||
float _lock_time{0.0f}; /**< Time passed when stick are at zero position. */
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user