fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 20:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManual: replace yaw with yaw_sp for rotation

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent ec79b2f8c8
commit 017576262c
3 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+2 -2
View File
@@ -94,8 +94,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
* thrust along xy is required. The maximum thrust along xy depends on the thrust
* setpoint along z-direction, which is computed in PositionControl.cpp.
*/
matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f};
sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * sp);
Vector3f sp{_sticks(0), _sticks(1), 0.0f};
sp = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * sp);
if (sp.length() > 1.0f) {
sp.normalize();
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+5 -6
View File
@@ -61,7 +61,7 @@ void FlightTaskManualPosition::_scaleSticks()
FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/
matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));
Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));
float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
@@ -70,13 +70,12 @@ void FlightTaskManualPosition::_scaleSticks()
}
/* Scale to velocity.*/
matrix::Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
/* Rotate setpoint into local frame. */
matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
_vel_sp(0) = vel_world(0);
_vel_sp(1) = vel_world(1);
Vector3f vel_local = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
_vel_sp(0) = vel_local(0);
_vel_sp(1) = vel_local(1);
}
void FlightTaskManualPosition::_updateXYlock()
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+2 -1
View File
@@ -84,7 +84,8 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
{
/* Rotate setpoint into local frame. */
matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f};
sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * sp);
sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw_sp)) * sp);
/* Ensure that maximum tilt is in [0.001, Pi] */
float tilt_max = math::constrain(math::radians(_tilt_max_man.get()), 0.001f, M_PI_F);