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

FlightTaskManual: transformation into heading frame use vehicle yaw or yaw_sp based

Browse Source 
on vehicle rotation in yaw
This commit is contained in:
Dennis Mannhart
2018-02-22 08:33:48 +01:00
committed by Beat Küng
parent ab8527cc8f
commit 421aeb69d8
4 changed files with 19 additions and 11 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.
*/
Vector3f sp{_sticks(0), _sticks(1), 0.0f};
sp = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * sp);
Vector2f sp{_sticks(0), _sticks(1)};
_rotateIntoHeadingFrame(sp);
if (sp.length() > 1.0f) {
sp.normalize();
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+3 -3
View File
@@ -73,9 +73,9 @@ void FlightTaskManualPosition::_scaleSticks()
Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
/* Rotate setpoint into local frame. */
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);
_rotateIntoHeadingFrame(vel_sp_xy);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
}
void FlightTaskManualPosition::_updateXYlock()
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+13 -6
View File
@@ -85,9 +85,8 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
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);
Vector2f sp{_sticks(0), _sticks(1)};
_rotateIntoHeadingFrame(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);
@@ -101,7 +100,7 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
* be captured through Axis-Angle.
*/
/* The Axis-Angle is the perpendicular vector to xy-stick input */
matrix::Vector2f v = matrix::Vector2f(y, -x);
Vector2f v = Vector2f(y, -x);
float v_norm = v.norm(); // the norm of v defines the tilt angle
if (v_norm > tilt_max) { // limit to the configured maximum tilt angle
@@ -111,8 +110,16 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
/* The final thrust setpoint is found by rotating the scaled unit vector pointing
* upward by the Axis-Angle.
*/
matrix::Quatf q_sp = matrix::AxisAnglef(v(0), v(1), 0.0f);
_thr_sp = q_sp.conjugate(matrix::Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
_thr_sp = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
}
void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)
{
float yaw_rotate = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw;
Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
v(0) = v_r(0);
v(1) = v_r(1);
}
void FlightTaskManualStabilized::_updateSetpoints()
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
+1
View File
@@ -56,6 +56,7 @@ public:
protected:
virtual void _updateSetpoints(); /**< updates all setpoints*/
virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
private: