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

FlightTaskManualAcceleration: smooth position unlock

Browse Source 
The velocity setpoint of the position controller
does a jump when unlocking position with a non-zero position error.

This is solved by using the velocity setpoint feedback to smoothly
take over.
This commit is contained in:
Matthias Grob 2021-02-08 18:36:18 +01:00 committed by Lorenz Meier
parent c9eac29d25
commit dec353219d
3 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

@ -70,7 +70,7 @@ bool FlightTaskManualAcceleration::update()
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
_deltatime);
Vector2f(_velocity_setpoint_feedback), _deltatime);
_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
_constraints.want_takeoff = _checkTakeoff();

13
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
View File

@ -66,7 +66,7 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
}
void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
const float dt)
const matrix::Vector2f &vel_sp_feedback, const float dt)
{
// maximum commanded acceleration and velocity
Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
@ -98,7 +98,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
// Generate velocity setpoint by forward integrating commanded acceleration
_velocity_setpoint += _acceleration_setpoint * dt;
lockPosition(pos, dt);
lockPosition(pos, vel_sp_feedback, dt);
}
void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
@ -147,7 +147,7 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
}
}
void StickAccelerationXY::lockPosition(const Vector3f &pos, const float dt)
void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt)
{
if (_velocity_setpoint.norm_squared() < FLT_EPSILON) {
if (!PX4_ISFINITE(_position_setpoint(0))) {
@ -155,7 +155,10 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const float dt)
}
} else {
_position_setpoint.setNaN();
if (PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint.setNaN();
// avoid velocity control jump because of remaining position error when unlocking
_velocity_setpoint = vel_sp_feedback;
}
}
}

4
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
View File

@ -56,7 +56,7 @@ public:
void resetVelocity(const matrix::Vector2f &velocity);
void resetAcceleration(const matrix::Vector2f &acceleration);
void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,
const float dt);
const matrix::Vector2f &vel_sp_feedback, const float dt);
void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
private:
@ -64,7 +64,7 @@ private:
matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
const matrix::Vector2f &vel_sp);
void applyTiltLimit(matrix::Vector2f &acceleration);
void lockPosition(const matrix::Vector3f &pos, const float dt);
void lockPosition(const matrix::Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt);
SlewRate<float> _acceleration_slew_rate_x;
SlewRate<float> _acceleration_slew_rate_y;