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: adapt position reset to new structure

Browse Source 
The mapping itself was seprated out into a calls because it was reused
for the experimental nudging implementation.
The position resets which were handled correctly before now
change the wrong setpoints and I adjusted.

The nudging has to be before any filtering, then these member setpoints
which are essentially copies are not needed anymore.
This commit is contained in:
Matthias Grob 2021-02-09 18:23:42 +01:00
parent c524e61b97
commit 7686533abb
2 changed files with 2 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,37 +79,10 @@ bool FlightTaskManualAcceleration::update()
void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY()
{
if (PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
}
_stick_acceleration_xy.resetPosition();
}
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY()
{
if (PX4_ISFINITE(_velocity_setpoint(0))) {
_velocity_setpoint(0) = _velocity(0);
_velocity_setpoint(1) = _velocity(1);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerPositionZ()
{
if (PX4_ISFINITE(_position_setpoint(2))) {
_position_setpoint(2) = _position(2);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityZ()
{
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) = _velocity(2);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi)
{
if (PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint += delta_psi;
}
_stick_acceleration_xy.resetVelocity(Vector2f(_velocity));
}

3
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp
View File

@ -58,7 +58,4 @@ private:
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
void _ekfResetHandlerHeading(float delta_psi) override;
};