Checked in L1 position and TECS altitude control. Not test-flown in HIL or outdoors yet

1 files changed, 236 insertions, 0 deletions

diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h
new file mode 100644
index 000000000..661651f08
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_control.h
@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * 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
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 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
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ *
+ * @author Lorenz Meier
+ *
+ * Acknowledgements and References:
+ *
+ * Original publication:
+ *    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
+ *
+ */
+
+#ifndef ECL_L1_POS_CONTROL_H
+#define ECL_L1_POS_CONTROL_H
+
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+
+/**
+ * L1 Nonlinear Guidance Logic
+ */
+class __EXPORT ECL_L1_Pos_Control
+{
+public:
+	ECL_L1_Pos_Control() {
+		_L1_period = 25;
+		_L1_damping = 0.75f;
+	}
+
+	/**
+	 * The current target bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float nav_bearing();
+
+	/**
+	 * Get lateral acceleration demand.
+	 *
+	 * @return Lateral acceleration in m/s^2
+	 */
+	float nav_lateral_acceleration_demand();
+
+	// return the heading error angle +ve to left of track
+
+
+	/**
+	 * Heading error.
+	 *
+	 * The heading error is either compared to the current track
+	 * or to the tangent of the current loiter radius.
+	 */
+	float bearing_error();
+
+
+	/**
+	 * Bearing from aircraft to current target.
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float target_bearing();
+
+
+	/**
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float nav_roll();
+
+
+	/**
+	 * Get the current crosstrack error.
+	 *
+	 * @return Crosstrack error in meters.
+	 */
+	float crosstrack_error();
+
+
+	/**
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reached_loiter_target();
+
+
+	/**
+	 * Get the switch distance
+	 * 
+	 * This is the distance at which the system will
+	 * switch to the next waypoint. This depends on the
+	 * period and damping
+	 *
+	 * @param waypoint_switch_radius The switching radius the waypoint has set.
+	 */
+	float switch_distance(float waypoint_switch_radius);
+
+
+	/**
+	 * Navigate between two waypoints
+	 *
+	 * 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.
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+			   const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Navigate on an orbit around a loiter waypoint.
+	 *
+	 * @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,
+			   const math::Vector2f &ground_speed_vector);
+
+
+	/**
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain direction and does not perform cross
+	 * track correction.
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Keep the wings level
+	 */
+	void navigate_level_flight(float current_heading);
+
+
+	/**
+	 * Set the L1 period.
+	 */
+	void set_l1_period(float period) {
+		_L1_period = period;
+		_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;
+	}
+
+
+	/**
+	 * Set the L1 damping factor.
+	 *
+	 * The original publication recommends a default of sqrt(2) / 2 = 0.707
+	 */
+	void set_l1_damping(float damping) {
+		_L1_damping = damping;
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		_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
+	bool _circle_mode;		///< flag for loiter mode
+	float _nav_bearing;		///< bearing to L1 reference point
+	float _bearing_error;		///< bearing error
+	float _crosstrack_error;	///< crosstrack error in meters
+	float _target_bearing;		///< the heading setpoint
+
+	float _L1_period;		///< L1 tracking period in seconds
+	float _L1_damping;		///< L1 damping ratio
+	float _L1_ratio;		///< L1 ratio for navigation
+	float _K_L1;			///< L1 control gain for _L1_damping
+	float _heading_omega;		///< Normalized frequency
+
+	/**
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+
+};
+
+
+#endif /* ECL_L1_POS_CONTROL_H */