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

FlightTaskAuto: triplet yaw-setpoint has priority over MPC_YAW_MODE.

Browse Source 
Add MPC_YAW_MODE option for heading along trajectory, which will be the same as
option 0 (heading towards target) if trajectory is straight line
This commit is contained in:
Dennis Mannhart
2018-08-02 09:50:59 +02:00
parent 694f49c80a
commit 02feb10865
2 changed files with 22 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+20 -15
View File
@@ -198,7 +198,13 @@ bool FlightTaskAuto::_evaluateTriplets()
_yaw_setpoint = NAN;
} else {
_set_heading_from_mode();
if (_sub_triplet_setpoint->get().current.yaw_valid) {
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
} else {
_set_heading_from_mode();
}
_yawspeed_setpoint = NAN;
}
@@ -220,17 +226,12 @@ void FlightTaskAuto::_set_heading_from_mode()
switch (MPC_YAW_MODE.get()) {
case 0: { // Heading is set by waypoint.
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
break;
}
case 1: { // Heading points towards the current waypoint.
case 0: { // Heading points towards the current waypoint.
v = Vector2f(&_target(0)) - Vector2f(&_position(0));
break;
}
case 2: { // Heading points towards home.
case 1: { // Heading points towards home.
if (_sub_home_position->get().valid_hpos) {
v = Vector2f(_sub_home_position->get().x, _sub_home_position->get().y) - Vector2f(&_position(0));
}
@@ -238,24 +239,28 @@ void FlightTaskAuto::_set_heading_from_mode()
break;
}
case 3: { // Heading point away from home.
case 2: { // Heading point away from home.
if (_sub_home_position->get().valid_hpos) {
v = Vector2f(&_position(0)) - Vector2f(_sub_home_position->get().x, _sub_home_position->get().y);
}
break;
}
case 3: { // Along trajectory.
// The heading depends on the kind of setpoint generation. This needs to be implemented
// in the subclasses where the velocity setpoints are generated.
v *= NAN;
}
}
// We only adjust yaw if vehicle is outside of acceptance radius.
// This prevents excessive yawing.
if (PX4_ISFINITE(v.length()) && v.length() > NAV_ACC_RAD.get()) {
v.normalize();
// To find yaw: take dot product of x = (1,0) and v
// and multiply by the sign given of cross product of x and v.
// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
_yaw_setpoint = math::sign(v(1)) * wrap_pi(acosf(v(0)));
_compute_heading_from_2D_vector(_yaw_setpoint, v);
} else {
_yaw_setpoint = NAN;
}
}
src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
+2 -2
View File
@@ -101,8 +101,8 @@ protected:
(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line
(ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE // defines how heading is executed
);
(ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE // defines how heading is executed
);
private:
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */