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: adapt tasks to member setpoints

Browse Source 
The FlightTaskManual... tasks already had their internal
setpoint member variables. I switched them to use the
architecture with setpoint member variables as it was
implemented the commit before. They simplify a lot.
This commit is contained in:
Matthias Grob 2018-02-28 10:02:20 +01:00 committed by Beat Küng
parent 309237c4a2
commit eaf4f99e38
10 changed files with 36 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/FlightTasks/tasks/FlightTaskManual.cpp
View File

@ -98,12 +98,3 @@ bool FlightTaskManual::_evaluateSticks()
return false;
}
}
void FlightTaskManual::_resetToNAN()
{
_thr_sp *= NAN;
_vel_sp *= NAN;
_pos_sp *= NAN;
_yaw_sp = NAN;
_yaw_rate_sp = NAN;
}

9
src/lib/FlightTasks/tasks/FlightTaskManual.hpp
View File

@ -63,15 +63,6 @@ protected:
matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
control::BlockParamFloat _stick_dz; /**< 0-deadzone around the center for the sticks */
/* Setpoints: NAN means that setpoint is not being considered. */
matrix::Vector3f _thr_sp{NAN, NAN, NAN}; /**< thrust setpoint */
matrix::Vector3f _vel_sp{NAN, NAN, NAN}; /**< velocity setpoint */
matrix::Vector3f _pos_sp{NAN, NAN, NAN}; /**< position setpoint */
float _yaw_sp{NAN}; /**< yaw setpoint */
float _yaw_rate_sp{NAN}; /**< yawspeed setpoint */
void _resetToNAN();
private:
uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};

23
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
View File

@ -48,13 +48,6 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent,
{}
bool FlightTaskManualAltitude::activate()
{
bool ret = FlightTaskManualStabilized::activate();
_vel_sp(2) = 0.0f;
return ret;
}
void FlightTaskManualAltitude::_scaleSticks()
{
/* Reuse same scaling as for stabilized */
@ -62,7 +55,7 @@ void FlightTaskManualAltitude::_scaleSticks()
/* Scale horizontal velocity with expo curve stick input*/
const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
_vel_sp(2) = vel_max_z * _sticks_expo(2);
_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
}
void FlightTaskManualAltitude::_updateAltitudeLock()
@ -72,14 +65,14 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
*/
/* handle position and altitude hold */
const bool apply_brake_z = fabsf(_vel_sp(2)) <= FLT_EPSILON;
const bool apply_brake_z = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON;
const bool stopped_z = (_vel_hold_thr_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_hold_thr_z.get());
if (apply_brake_z && stopped_z && !PX4_ISFINITE(_pos_sp(2))) {
_pos_sp(2) = _position(2);
if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
_position_setpoint(2) = _position(2);
} else if (!apply_brake_z) {
_pos_sp(2) = NAN;
_position_setpoint(2) = NAN;
}
}
@ -87,7 +80,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
{
FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints
_thr_sp *= NAN; // Don't need thrust setpoint from Stabilized mode.
_thrust_setpoint *= NAN; // Don't need thrust setpoint from Stabilized mode.
/* Thrust in xy are extracted directly from stick inputs. A magnitude of
* 1 means that maximum thrust along xy is required. A magnitude of 0 means no
@ -101,8 +94,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
sp.normalize();
}
_thr_sp(0) = sp(0);
_thr_sp(1) = sp(1);
_thrust_setpoint(0) = sp(0);
_thrust_setpoint(1) = sp(1);
_updateAltitudeLock();
}

2
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
View File

@ -48,8 +48,6 @@ public:
virtual ~FlightTaskManualAltitude() = default;
bool activate() override;
protected:
control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */

2
src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
View File

@ -50,7 +50,7 @@ void FlightTaskManualAltitudeSmooth::_updateSetpoints()
FlightTaskManualAltitude::_updateSetpoints();
/* Smooth velocity in z*/
_smoothing.smoothVelFromSticks(_vel_sp(2), _deltatime);
_smoothing.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
/* Check for altitude lock*/
_updateAltitudeLock();

25
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
View File

@ -48,13 +48,6 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent,
_vel_hold_thr_xy(parent, "MPC_HOLD_MAX_XY", false)
{}
bool FlightTaskManualPosition::activate()
{
bool ret = FlightTaskManualAltitude::activate();
_vel_sp(0) = _vel_sp(1) = 0.0f;
return ret;
}
void FlightTaskManualPosition::_scaleSticks()
{
/* Use same scaling as for FlightTaskManualAltitude */
@ -74,31 +67,31 @@ void FlightTaskManualPosition::_scaleSticks()
/* Rotate setpoint into local frame. */
_rotateIntoHeadingFrame(vel_sp_xy);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
_velocity_setpoint(0) = vel_sp_xy(0);
_velocity_setpoint(1) = vel_sp_xy(1);
}
void FlightTaskManualPosition::_updateXYlock()
{
/* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(&_velocity(0)).length();
const bool apply_brake = Vector2f(&_vel_sp(0)).length() < FLT_EPSILON;
const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp(0))) {
_pos_sp(0) = _position(0);
_pos_sp(1) = _position(1);
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
} else if (!apply_brake) {
/* don't lock*/
_pos_sp(0) = NAN;
_pos_sp(1) = NAN;
_position_setpoint(0) = NAN;
_position_setpoint(1) = NAN;
}
}
void FlightTaskManualPosition::_updateSetpoints()
{
FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
_thr_sp *= NAN; // don't require any thrust setpoints
_thrust_setpoint *= NAN; // don't require any thrust setpoints
_updateXYlock(); // check for position lock
}

2
src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
View File

@ -49,8 +49,6 @@ public:
virtual ~FlightTaskManualPosition() = default;
bool activate() override;
protected:
control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
control::BlockParamFloat _acc_xy_max;/**< maximum acceleration horizontally. Only used to compute lock time */

10
src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
View File

@ -52,16 +52,16 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
/* Smooth velocity setpoint in xy.*/
matrix::Vector2f vel(&_velocity(0));
Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yaw_rate_sp, _deltatime);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
_velocity_setpoint(0) = vel_sp_xy(0);
_velocity_setpoint(1) = vel_sp_xy(1);
/* Check for altitude lock.*/
_updateXYlock();
/* Smooth velocity in z.*/
_smoothingZ.smoothVelFromSticks(_vel_sp(2), _deltatime);
_smoothingZ.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
/* Check for altitude lock*/
_updateAltitudeLock();

25
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
View File

@ -52,9 +52,7 @@ FlightTaskManualStabilized::FlightTaskManualStabilized(control::SuperBlock *pare
bool FlightTaskManualStabilized::activate()
{
_resetToNAN();
_yaw_rate_sp = 0.0f;
_thr_sp = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
return FlightTaskManual::activate();
}
@ -62,8 +60,9 @@ void FlightTaskManualStabilized::_scaleSticks()
{
/* Scale sticks to yaw and thrust using
* linear scale for yaw and piecewise linear map for thrust. */
_yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
_yaw_rate_sp = math::sign(_yaw_rate_sp) * math::min(fabsf(_yaw_rate_sp), math::radians(_yaw_rate_max.get()));
_yawspeed_setpoint = _sticks(3) * math::radians(_yaw_rate_scaling.get());
_yawspeed_setpoint = math::sign(_yawspeed_setpoint) * math::min(fabsf(_yawspeed_setpoint),
math::radians(_yaw_rate_max.get()));
_throttle = _throttleCurve();
}
@ -74,11 +73,11 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
* TODO: add yawspeed to get threshold.*/
const bool stick_yaw_zero = fabsf(_sticks(3)) <= _stick_dz.get();
if (stick_yaw_zero && !PX4_ISFINITE(_yaw_sp)) {
_yaw_sp = _yaw;
if (stick_yaw_zero && !PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint = _yaw;
} else if (!stick_yaw_zero) {
_yaw_sp = NAN;
_yaw_setpoint = NAN;
}
}
@ -111,12 +110,12 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
* upward by the Axis-Angle.
*/
Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
_thr_sp = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
_thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
}
void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)
{
float yaw_rotate = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw;
float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
v(0) = v_r(0);
v(1) = v_r(1);
@ -147,11 +146,5 @@ bool FlightTaskManualStabilized::update()
_scaleSticks();
_updateSetpoints();
_position_setpoint = _pos_sp;
_velocity_setpoint = _vel_sp;
_thrust_setpoint = _thr_sp;
_yaw_setpoint = _yaw_sp;
_yawspeed_setpoint = _yaw_rate_sp;
return true;
}

8
src/lib/FlightTasks/tasks/FlightTaskSport.hpp
View File

@ -61,11 +61,11 @@ protected:
FlightTaskManualPosition::_updateSetpoints(); // get all setpoints from position task
/* Scale horizontal velocity setpoint by maximum allowed velocity. */
if (PX4_ISFINITE(_vel_sp(0)) && Vector2f(&_vel_sp(0)).length() > 0.0f) {
Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
if (PX4_ISFINITE(_velocity_setpoint(0)) && Vector2f(&_velocity_setpoint(0)).length() > 0.0f) {
Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
vel_sp_xy = vel_sp_xy.normalized() * _vel_xy_max.get() / _vel_xy_manual_max.get() * vel_sp_xy.length();
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
_velocity_setpoint(0) = vel_sp_xy(0);
_velocity_setpoint(1) = vel_sp_xy(1);
}
}