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

Compile fixes, cleanups, better references

Browse Source
This commit is contained in:
Lorenz Meier 2013-09-10 20:26:50 +02:00
parent a2ca46d454
commit 4e0c5f9489
3 changed files with 39 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/ecl/l1/ecl_l1_pos_control.cpp
View File

@ -6,9 +6,9 @@
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* 1. Redistributions of source code must retain the vector_ABove copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* 2. Redistributions in binary form must reproduce the vector_ABove copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
@ -18,14 +18,14 @@
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTvector_ABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIvector_ABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* AND ON ANY THEORY OF LIvector_ABILITY, WHETHER IN CONTRACT, STRICT
* LIvector_ABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*

42
src/lib/ecl/l1/ecl_l1_pos_control.h
View File

@ -39,18 +39,22 @@
*
* Acknowledgements and References:
*
* Original publication:
* S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
* This implementation has been built for PX4 based on the original
* publication from [1] and does include a lot of the ideas (not code)
* from [2].
*
*
* [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Original navigation logic modified by Paul Riseborough 2013:
* - Explicit control over frequency and damping
* - Explicit control over track capture angle
* - Ability to use loiter radius smaller than L1 length
* - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
* - Modified to enable period and damping of guidance loop to be set explicitly
* - Modified to provide explicit control over capture angle
* [2] Paul Riseborough and Andrew Tridgell, L1 control for APM. Aug 2013.
* - Explicit control over frequency and damping
* - Explicit control over track capture angle
* - Ability to use loiter radius smaller than L1 length
* - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
* - Modified to enable period and damping of guidance loop to be set explicitly
* - Modified to provide explicit control over capture angle
*
*/
@ -79,6 +83,7 @@ public:
*/
float nav_bearing();
/**
* Get lateral acceleration demand.
*
@ -86,8 +91,6 @@ public:
*/
float nav_lateral_acceleration_demand();
// return the heading error angle +ve to left of track
/**
* Heading error.
@ -146,6 +149,7 @@ public:
* Calling this function with two waypoints results in the
* control outputs to fly to the line segment defined by
* the points and once captured following the line segment.
* This follows the logic in [1].
*
* @return sets _lateral_accel setpoint
*/
@ -156,6 +160,9 @@ public:
/**
* Navigate on an orbit around a loiter waypoint.
*
* This allow orbits smaller than the L1 length,
* this modification was introduced in [2].
*
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
@ -166,7 +173,8 @@ public:
* Navigate on a fixed bearing.
*
* This only holds a certain direction and does not perform cross
* track correction.
* track correction. Helpful for semi-autonomous modes. Introduced
* by [2].
*
* @return sets _lateral_accel setpoint
*/
@ -174,7 +182,10 @@ public:
/**
* Keep the wings level
* Keep the wings level.
*
* This is typically needed for maximum-lift-demand situations,
* such as takeoff or near stall. Introduced in [2].
*/
void navigate_level_flight(float current_heading);
@ -184,6 +195,7 @@ public:
*/
void set_l1_period(float period) {
_L1_period = period;
/* calculate the ratio introduced in [2] */
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
/* calculate normalized frequency for heading tracking */
_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
@ -197,14 +209,16 @@ public:
*/
void set_l1_damping(float damping) {
_L1_damping = damping;
/* calculate the ratio introduced in [2] */
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
/* calculate the L1 gain (following [2]) */
_K_L1 = 4.0f * _L1_damping * _L1_damping;
}
private:
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
float _L1_distance; ///< L1 lead distance, defined by period and damping
float _L1_distance; ///< L1 lead distance, defined by period and damping
bool _circle_mode; ///< flag for loiter mode
float _nav_bearing; ///< bearing to L1 reference point
float _bearing_error; ///< bearing error

8
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -36,14 +36,16 @@
* Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll
* angle, equivalent to a lateral motion (for copters and rovers).
*
* Original publication for horizontal control:
* Original publication for horizontal control class:
* S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Original implementation for total energy control:
* Original implementation for total energy control class:
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
*
* More details and acknowledgements in the referenced library headers.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
@ -79,7 +81,7 @@
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <ecl/l1/ECL_L1_Pos_Control.h>
#include <ecl/l1/ecl_l1_pos_control.h>
#include <external_lgpl/tecs/tecs.h>
/**