diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-10 11:29:05 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-10 11:53:59 +0200 |
commit | 12e4e393bdedc4a8f929bff8bff73b00e35752dd (patch) | |
tree | 3bac4d2a68af61ab3988d66a6f98450cef09b00d /src/lib/ecl | |
parent | 5182860a68fe5733062bd342fbe85310497e20d3 (diff) | |
download | px4-firmware-12e4e393bdedc4a8f929bff8bff73b00e35752dd.tar.gz px4-firmware-12e4e393bdedc4a8f929bff8bff73b00e35752dd.tar.bz2 px4-firmware-12e4e393bdedc4a8f929bff8bff73b00e35752dd.zip |
Checked in L1 position and TECS altitude control. Not test-flown in HIL or outdoors yet
Diffstat (limited to 'src/lib/ecl')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 2 | ||||
-rw-r--r-- | src/lib/ecl/ecl.h | 1 | ||||
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_control.cpp | 323 | ||||
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_control.h | 236 | ||||
-rw-r--r-- | src/lib/ecl/module.mk | 5 |
10 files changed, 569 insertions, 8 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d0b5fcab7..44b339ce5 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 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. * diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 02ca62dad..7fbfd6fbc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 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. * diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6de30fc33..f3a8585ae 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 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. * diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 7fbcdf4b8..3427b67c2 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 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. * diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a0f901e71..2b7429996 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 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. * diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index cfaa6c233..66b227918 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 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. * diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index 2bd76ce97..e0f207696 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,7 @@ */ #include <drivers/drv_hrt.h> +#include <geo/geo.h> #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp new file mode 100644 index 000000000..533409217 --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -0,0 +1,323 @@ +/**************************************************************************** + * + * 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 vector_ABove copyright + * notice, this list of conditions and the following disclaimer. + * 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. + * 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 MERCHANTvector_ABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * 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 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. + * + ****************************************************************************/ + +/** + * @file ecl_l1_pos_control.h + * Implementation of L1 position control. + * Authors and acknowledgements in header. + * + */ + +#include "ecl_l1_pos_control.h" + +float ECL_L1_Pos_Control::nav_roll() +{ + float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); + ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); + return ret; +} + +float ECL_L1_Pos_Control::nav_lateral_acceleration_demand() +{ + return _lateral_accel; +} + +float ECL_L1_Pos_Control::nav_bearing() +{ + return _wrap_pi(_nav_bearing); +} + +float ECL_L1_Pos_Control::bearing_error() +{ + return _bearing_error; +} + +float ECL_L1_Pos_Control::target_bearing() +{ + return _target_bearing; +} + +float ECL_L1_Pos_Control::switch_distance(float wp_radius) +{ + return math::min(wp_radius, _L1_distance); +} + +bool ECL_L1_Pos_Control::reached_loiter_target(void) +{ + return _circle_mode; +} + +float ECL_L1_Pos_Control::crosstrack_error(void) +{ + return _crosstrack_error; +} + +void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed_vector) +{ + + float eta; + float xtrack_vel; + float ltrack_vel; + + /* get the direction between the last (visited) and next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + + /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length(), 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate vector from A to B */ + math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + + /* + * check if waypoints are on top of each other. If yes, + * skip A and directly continue to B + */ + if (vector_AB.length() < 1.0e-6f) { + vector_AB = get_local_planar_vector(vector_curr_position, vector_B); + } + + vector_AB.normalize(); + + /* calculate the vector from waypoint A to the aircraft */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* calculate crosstrack error (output only) */ + _crosstrack_error = vector_AB % vector_A_to_airplane; + + /* + * If the current position is in a +-135 degree angle behind waypoint A + * and further away from A than the L1 distance, then A becomes the L1 point. + * If the aircraft is already between A and B normal L1 logic is applied. + */ + float distance_A_to_airplane = vector_A_to_airplane.length(); + float alongTrackDist = vector_A_to_airplane * vector_AB; + + if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { + + /* calculate eta to fly to waypoint A */ + + /* unit vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + } else { + + /* calculate eta to fly along the line between A and B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % vector_AB; + /* velocity along line */ + ltrack_vel = ground_speed_vector * vector_AB; + /* calculate eta2 (angle of velocity vector relative to line) */ + float eta2 = atan2f(xtrack_vel, ltrack_vel); + /* calculate eta1 (angle to L1 point) */ + float xtrackErr = vector_A_to_airplane % vector_AB; + float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); + /* limit output to 45 degrees */ + sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f); + float eta1 = asinf(sine_eta1); + eta = eta1 + eta2; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + } + + /* limit angle to +-90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* flying to waypoints, not circling them */ + _circle_mode = false; + + /* the bearing angle, in NED frame */ + _bearing_error = eta; +} + +void ECL_L1_Pos_Control::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) +{ + + /* calculate the gains for the PD loop (circle tracking) */ + float omega = (2.0f * M_PI_F / _L1_period); + float K_crosstrack = omega * omega; + float K_velocity = 2.0f * _L1_damping * omega; + + /* update bearing to next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + + /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length() , 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate the vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* store the normalized vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + + /* calculate eta angle towards the loiter center */ + + /* velocity across / orthogonal to line from waypoint to current position */ + float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; + /* velocity along line from waypoint to current position */ + float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); + float eta = atan2f(xtrack_vel_center, ltrack_vel_center); + /* limit eta to 90 degrees */ + eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); + + /* calculate the lateral acceleration to capture the center point */ + float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* for PD control: Calculate radial position and velocity errors */ + + /* radial velocity error */ + float xtrack_vel_circle = -ltrack_vel_center; + /* radial distance from the loiter circle (not center) */ + float xtrack_err_circle = vector_A_to_airplane.length() - radius; + + /* cross track error for feedback */ + _crosstrack_error = xtrack_err_circle; + + /* calculate PD update to circle waypoint */ + float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); + + /* calculate velocity on circle / along tangent */ + float tangent_vel = xtrack_vel_center * loiter_direction; + + /* prevent PD output from turning the wrong way */ + if (tangent_vel < 0.0f) { + lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); + } + + /* calculate centripetal acceleration setpoint */ + float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); + + /* add PD control on circle and centripetal acceleration for total circle command */ + float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); + + /* + * Switch between circle (loiter) and capture (towards waypoint center) mode when + * the commands switch over. Only fly towards waypoint if outside the circle. + */ + + // XXX check switch over + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { + _lateral_accel = lateral_accel_sp_center; + _circle_mode = false; + /* angle between requested and current velocity vector */ + _bearing_error = eta; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + } else { + _lateral_accel = lateral_accel_sp_circle; + _circle_mode = true; + _bearing_error = 0.0f; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } +} + + +void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +{ + + float eta; + + /* + * As the commanded heading is the only reference + * (and no crosstrack correction occurs), + * target and navigation bearing become the same + */ + _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); + eta = _target_bearing - _wrap_pi(current_heading); + eta = _wrap_pi(eta); + + /* consequently the bearing error is exactly eta: */ + _bearing_error = eta; + + /* ground speed is the length of the ground speed vector */ + float ground_speed = ground_speed_vector.length(); + + /* adjust L1 distance to keep constant frequency */ + _L1_distance = ground_speed / _heading_omega; + float omega_vel = ground_speed * _heading_omega; + + /* not circling a waypoint */ + _circle_mode = false; + + /* navigating heading means by definition no crosstrack error */ + _crosstrack_error = 0; + + /* limit eta to 90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = 2.0f * sinf(eta) * omega_vel; +} + +void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) +{ + /* reset all heading / error measures resulting in zero roll */ + _target_bearing = current_heading; + _nav_bearing = current_heading; + _bearing_error = 0; + _crosstrack_error = 0; + _lateral_accel = 0; + + /* not circling a waypoint when flying level */ + _circle_mode = false; + +} + + +math::Vector2f ECL_L1_Pos_Control::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +{ + math::Vector2f out; + + out.setX(math::radians((target.getX() - origin.getX()))); + out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + + return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH); +} + 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 */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index 19610872e..e8bca3c76 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -36,5 +36,6 @@ # SRCS = attitude_fw/ecl_pitch_controller.cpp \ - attitude_fw/ecl_roll_controller.cpp \ - attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_roll_controller.cpp \ + attitude_fw/ecl_yaw_controller.cpp \ + l1/ecl_l1_pos_control.cpp |