FlightTaskTransition: don't overwrite setpoints with estimates

Previously acceleration setpoints were not executed and just used
to pass a possible rough initialization value for the next task. Now
they get executed by the multicopter controller and hence
overwriting them with rough estimates doesn't work anymore.
This commit is contained in:
Matthias Grob
2020-03-24 17:25:09 +01:00
parent 58fd3e2ddc
commit f6ceb08522
2 changed files with 0 additions and 20 deletions
@@ -47,8 +47,6 @@ bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s last_setpo
checkSetpoints(last_setpoint);
_transition_altitude = last_setpoint.z;
_transition_yaw = last_setpoint.yaw;
_acceleration_setpoint.setAll(0.f);
_velocity_prev = _velocity;
return FlightTask::activate(last_setpoint);
}
@@ -60,29 +58,12 @@ void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &set
if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
}
void FlightTaskTransition::updateAccelerationEstimate()
{
// Estimate the acceleration by filtering the raw derivative of the velocity estimate
// This is done to provide a good estimate of the current acceleration to the next flight task after back-transition
_acceleration_setpoint = .9f * _acceleration_setpoint + .1f * (_velocity - _velocity_prev) / _deltatime;
if (!PX4_ISFINITE(_acceleration_setpoint(0)) ||
!PX4_ISFINITE(_acceleration_setpoint(1)) ||
!PX4_ISFINITE(_acceleration_setpoint(2))) {
_acceleration_setpoint.setZero();
}
_velocity_prev = _velocity;
}
bool FlightTaskTransition::update()
{
// level wings during the transition, altitude should be controlled
_position_setpoint(2) = _transition_altitude;
_acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
updateAccelerationEstimate();
_yaw_setpoint = _transition_yaw;
return true;
}
@@ -57,5 +57,4 @@ private:
float _transition_altitude = 0.0f;
float _transition_yaw = 0.0f;
matrix::Vector3f _velocity_prev{};
};