yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

SmoothVel - Fix altitude lock logic.

Browse Source 
It was broken because _velocity_setpoint is used for input and output
and was assumed to be the input at a place where it was already overwitten
To clarify this, the input setpoint is renamed "target"
This commit is contained in:
bresch
2019-08-22 15:54:35 +02:00
committed by Julian Oes
parent dd2d5c029f
commit f4b40865af
2 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

@@ -130,7 +130,9 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
* is used to set current velocity of the trajectory. * is used to set current velocity of the trajectory.
*/ */
if (fabsf(_velocity_setpoint(2)) > FLT_EPSILON) { const float velocity_target_z = _velocity_setpoint(2);
if (fabsf(velocity_target_z) > FLT_EPSILON) {
if (_position_lock_z_active) { if (_position_lock_z_active) {
_smoothing.setCurrentVelocity(_velocity_setpoint_feedback( _smoothing.setCurrentVelocity(_velocity_setpoint_feedback(
2)); // Start the trajectory at the current velocity setpoint 2)); // Start the trajectory at the current velocity setpoint
@@ -159,7 +161,7 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
if (fabsf(_vel_sp_smooth) < 0.1f && if (fabsf(_vel_sp_smooth) < 0.1f &&
fabsf(_acceleration_setpoint(2)) < .2f && fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(_velocity_setpoint(2)) <= FLT_EPSILON) { fabsf(velocity_target_z) <= FLT_EPSILON) {
_position_lock_z_active = true; _position_lock_z_active = true;
} }

11
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@@ -169,9 +169,10 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
* and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller
* is used to set current velocity of the trajectory. * is used to set current velocity of the trajectory.
*/ */
Vector2f velocity_setpoint_xy = Vector2f(&_velocity_setpoint(0)); const Vector2f velocity_target_xy = Vector2f(_velocity_setpoint);
const float velocity_target_z = _velocity_setpoint(2);
if (velocity_setpoint_xy.length() > FLT_EPSILON) { if (velocity_target_xy.length() > FLT_EPSILON) {
if (_position_lock_xy_active) { if (_position_lock_xy_active) {
_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback( _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(
0)); // Start the trajectory at the current velocity setpoint 0)); // Start the trajectory at the current velocity setpoint
@@ -183,7 +184,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_position_lock_xy_active = false; _position_lock_xy_active = false;
} }
if (fabsf(_velocity_setpoint(2)) > FLT_EPSILON) { if (fabsf(velocity_target_z) > FLT_EPSILON) {
if (_position_lock_z_active) { if (_position_lock_z_active) {
_smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback( _smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback(
2)); // Start the trajectory at the current velocity setpoint 2)); // Start the trajectory at the current velocity setpoint
@@ -233,13 +234,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
// Check for position lock transition // Check for position lock transition
if (Vector2f(_vel_sp_smooth).length() < 0.1f && if (Vector2f(_vel_sp_smooth).length() < 0.1f &&
Vector2f(_acceleration_setpoint).length() < .2f && Vector2f(_acceleration_setpoint).length() < .2f &&
velocity_setpoint_xy.length() <= FLT_EPSILON) { velocity_target_xy.length() <= FLT_EPSILON) {
_position_lock_xy_active = true; _position_lock_xy_active = true;
} }
if (fabsf(_vel_sp_smooth(2)) < 0.1f && if (fabsf(_vel_sp_smooth(2)) < 0.1f &&
fabsf(_acceleration_setpoint(2)) < .2f && fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(_velocity_setpoint(2)) <= FLT_EPSILON) { fabsf(velocity_target_z) <= FLT_EPSILON) {
_position_lock_z_active = true; _position_lock_z_active = true;
} }