Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

FlightTaskAuto: use longerThan() when possible in vector calculations

Browse Source
This commit is contained in:
Matthias Grob
2022-01-10 16:53:33 +01:00
parent 4fc7348582
commit 443406ea2b
1 changed files with 11 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+11 -12
View File
@@ -40,8 +40,6 @@
using namespace matrix;
static constexpr float SIGMA_NORM = 0.001f;
FlightTaskAuto::FlightTaskAuto() :
_obstacle_avoidance(this),
_sticks(this),
@@ -536,17 +534,18 @@ void FlightTaskAuto::_set_heading_from_mode()
break;
}
if (PX4_ISFINITE(v.length())) {
if (PX4_ISFINITE(v.norm_squared())) {
// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
// radius, lock yaw to current yaw.
// This prevents excessive yawing.
if (!_yaw_lock) {
if (v.length() < _target_acceptance_radius) {
if (v.longerThan(_target_acceptance_radius)) {
_compute_heading_from_2D_vector(_yaw_setpoint, v);
} else {
_yaw_setpoint = _yaw;
_yaw_lock = true;
} else {
_compute_heading_from_2D_vector(_yaw_setpoint, v);
}
}
@@ -630,11 +629,11 @@ State FlightTaskAuto::_getCurrentState()
// Target is behind.
return_state = State::target_behind;
} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _target_acceptance_radius) {
} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) {
// Current position is more than cruise speed in front of previous setpoint.
return_state = State::previous_infront;
} else if ((_position - _closest_pt).length() > _target_acceptance_radius) {
} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
// Vehicle is more than cruise speed off track.
return_state = State::offtrack;
@@ -683,13 +682,13 @@ void FlightTaskAuto::_updateInternalWaypoints()
bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
{
if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
if (PX4_ISFINITE(v.norm_squared()) && v.longerThan(1e-3f)) {
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)
heading = sign(v(1)) * wrap_pi(acosf(v(0)));
heading = sign(v(1)) * wrap_pi(acosf(v(0)));
return true;
}
@@ -758,8 +757,8 @@ bool FlightTaskAuto::_generateHeadingAlongTraj()
Vector2f vel_sp_xy(_velocity_setpoint);
Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
if ((vel_sp_xy.length() > .1f) &&
(traj_to_target.length() > 2.f)) {
if ((vel_sp_xy.longerThan(.1f)) &&
(traj_to_target.longerThan(2.f))) {
// Generate heading from velocity vector, only if it is long enough
// and if the drone is far enough from the target
_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);