mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 11:37:34 +08:00
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:
@@ -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{};
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user