yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: set heading based on yaw mode

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-23 15:32:23 +02:00
parent f0282bcd8f
commit 6cd16f345e
1 changed files with 52 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
View File

@@ -101,6 +101,7 @@ bool FlightTaskAuto::_evaluateTriplets()
// takeoff/land was initiated. Until then we do this kind of logic here.
// Check if triplet is valid. There must be at least a valid altitude.
if (!_sub_triplet_setpoint->get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) {
// Best we can do is to just set all waypoints to current state and return false.
_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
@@ -191,12 +192,14 @@ bool FlightTaskAuto::_evaluateTriplets()
}
}
// Check if yaw target is valid.
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
// set heading
if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) {
_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
_yaw_setpoint = NAN;
} else {
_set_heading_from_mode();
_yawspeed_setpoint = NAN;
}
// Calculate the current vehicle state and check if it has updated.
@@ -210,6 +213,52 @@ bool FlightTaskAuto::_evaluateTriplets()
return true;
}
void FlightTaskAuto::_set_heading_from_mode()
{
matrix::Vector2f v; // Vector that points towards desired location
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.
v = Vector2f(&_target(0)) - Vector2f(&_position(0));
break;
}
case 2: { // 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));
}
break;
}
case 3: { // 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;
}
}
// 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)));
}
}
bool FlightTaskAuto::_isFinite(const position_setpoint_s sp)
{
return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));