Compile fixes, cleanups, better references

2 files changed, 34 insertions, 20 deletions

diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp
index 6f5065960..533409217 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp
@@ -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.
  *
diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h
index 661651f08..8bb94d3eb 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_control.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_control.h
@@ -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