From 816f2d12d5a48c57c0223b006cb8c702cda2e94e Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 8 Feb 2018 11:23:13 +0100 Subject: [PATCH] FlightTaskAuto minor clean up --- src/lib/FlightTasks/tasks/FlightTask.hpp | 2 -- src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 1 - .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 17 ++++++++--------- .../FlightTasks/tasks/FlightTaskAutoLine.hpp | 10 +++++----- src/modules/mc_pos_control/PositionControl.cpp | 1 - .../mc_pos_control/Utility/ControlMath.cpp | 2 +- 6 files changed, 14 insertions(+), 19 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 43357cdc70..bb0a824e54 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -48,8 +48,6 @@ #include #include #include - - #include "../SubscriptionArray.hpp" diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 450ec38cd8..4725a0ca36 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -128,7 +128,6 @@ bool FlightTaskAuto::_evaluateTriplets() _sub_triplet_setpoint->get().previous.lon, &_prev_wp(0), &_prev_wp(1)); _prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude); - } else { _prev_wp = _position; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 1d68636a2a..aaaa0f169f 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -144,7 +144,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints() _next_wp = _target; } - /* Adjust destination and origin based on current vehicle state */ + /* Adjust destination and origin based on current vehicle state. */ Vector2f u_prev_to_target = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero(); Vector2f pos_to_target = Vector2f(&(_target - _position)(0)); Vector2f prev_to_pos = Vector2f(&(_position - _prev_wp)(0)); @@ -152,7 +152,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints() if (u_prev_to_target * pos_to_target < 0.0f) { - /* Target is behind */ + /* Target is behind. */ if (_current_state != State::target_behind) { _destination = _target; @@ -177,7 +177,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints() } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { - /* current position more than cruise speed in front of previous setpoint. */ + /* Current position is more than cruise speed in front of previous setpoint. */ if (_current_state != State::previous_infront) { _destination = _prev_wp; _origin = _position; @@ -201,7 +201,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints() } else if (Vector2f(Vector2f(&_position(0)) - closest_pt).length() > _mc_cruise_speed) { - /* Vehicle is more than cruise speed off track */ + /* Vehicle is more than cruise speed off track. */ if (_current_state != State::offtrack) { _destination = matrix::Vector3f(closest_pt(0), closest_pt(1), _target(2)); _origin = _position; @@ -226,6 +226,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints() } else { if ((_target - _destination).length() > 0.01f) { + /* A new target is available. Update speed at target.*/ _destination = _target; _origin = _prev_wp; _current_state = State::none; @@ -233,7 +234,6 @@ void FlightTaskAutoLine::_updateInternalWaypoints() float angle = 2.0f; _speed_at_target = 0.0f; - /* angle = cos(x) + 1.0 * angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */ if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f && @@ -251,19 +251,18 @@ void FlightTaskAutoLine::_updateInternalWaypoints() void FlightTaskAutoLine::_generateXYsetpoints() { - Vector2f pos_sp_to_dest = Vector2f(&_target(0)) - _pos_sp_xy; const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < _nav_rad.get(); if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _nav_rad.get()) || (!has_reached_altitude && pos_sp_to_dest.length() < _nav_rad.get())) { - /* Vehicle reached target in xy. Lock position */ + /* Vehicle reached target in xy and does not want to pass. Lock position */ _pos_sp_xy = Vector2f(&_destination(0)); _vel_sp_xy.zero(); } else { - + /* Get various path specific vectors. */ Vector2f u_prev_to_dest = Vector2f(&(_destination - _origin)(0)).unit_or_zero(); Vector2f prev_to_pos(&(_position - _origin)(0)); Vector2f closest_pt = Vector2f(&_origin(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); @@ -279,6 +278,7 @@ void FlightTaskAutoLine::_generateXYsetpoints() const float threshold_max = target_threshold; if (target_threshold < 0.5f * prev_to_dest.length()) { + /* Target threshold can not be more than distance from previous to target */ target_threshold = 0.5f * prev_to_dest.length(); } @@ -351,7 +351,6 @@ void FlightTaskAutoLine::_generateXYsetpoints() void FlightTaskAutoLine::_generateAltitudeSetpoints() { - /* Total distance between previous and target setpoint */ const float dist = fabsf(_destination(2) - _origin(2)); diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index e3a8ca3f0d..226a7da647 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -89,9 +89,9 @@ protected: void _generateLandSetpoints(); void _generateVelocitySetpoints(); void _generateTakeoffSetpoints(); - void _updateInternalWaypoints(); - void _generateSetpoints(); /**< Generate setpoints during auto tracking */ - void _generateAltitudeSetpoints(); - void _generateXYsetpoints(); - float _getVelcoityFromAngle(const float angle); /** Computes the speed at target depending on angle */ + void _updateInternalWaypoints(); /* Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ + void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */ + void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */ + void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */ + float _getVelcoityFromAngle(const float angle); /** Computes the speed at target depending on angle. */ }; diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index f4f9580d37..a287defcf2 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -109,7 +109,6 @@ void PositionControl::generateThrustYawSetpoint(const float &dt) if (!_skipController) { _positionController(); _velocityController(dt); - } } diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index 116ec3f56f..5f32cabded 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -114,7 +114,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con return att_sp; } -/* The sum of two vectors are constraints such that v0 has priority over v1. +/* The sum of two vectors are constraint such that v0 has priority over v1. * This means that if the length of v0+v1 exceeds max, then it is constraint such * that that v0 has priority. * Inputs: