From feec8b203675423dc843a65f08de198615217795 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 2 Mar 2023 17:05:45 +0100 Subject: [PATCH] L1: remove some functions that Rover doesn't need Signed-off-by: Silvan Fuhrer --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 195 ------------------ src/lib/l1/ECL_L1_Pos_Controller.hpp | 90 -------- .../RoverPositionControl.cpp | 4 - 3 files changed, 289 deletions(-) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index 8ec4f83d92..8d2c0cda37 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -46,25 +46,7 @@ #include -using matrix::Vector2d; using matrix::Vector2f; -using matrix::wrap_pi; - -void ECL_L1_Pos_Controller::update_roll_setpoint() -{ - float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); - roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad); - - if (_dt > 0.0f && _roll_slew_rate > 0.0f) { - // slew rate limiting active - roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt); - } - - if (PX4_ISFINITE(roll_new)) { - _roll_setpoint = roll_new; - } - -} float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { @@ -76,8 +58,6 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) { - _has_guidance_updated = true; - /* this follows the logic presented in [1] */ float eta = 0.0f; @@ -202,181 +182,6 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector 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; - - update_roll_setpoint(); -} - -void -ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius, - const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector) -{ - _has_guidance_updated = true; - - const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f; - - /* the complete guidance logic in this section was proposed by [2] */ - - /* 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; - - /* 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 */ - Vector2f vector_A_to_airplane = vector_curr_position - vector_A; - - Vector2f vector_A_to_airplane_unit; - - /* prevent NaN when normalizing */ - if (vector_A_to_airplane.length() > FLT_EPSILON) { - /* store the normalized vector from waypoint A to current position */ - vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - - } else { - vector_A_to_airplane_unit = vector_A_to_airplane; - } - - /* update bearing to next waypoint */ - _target_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); - - /* 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_multiplier; - - /* prevent PD output from turning the wrong way when in circle mode */ - const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction - - if (tangent_vel < -l1_op_tan_vel && _circle_mode) { - 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_multiplier * (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_counter_clockwise - && xtrack_err_circle > 0.0f) - || - (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && 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(1), -vector_A_to_airplane_unit(0)); - - } 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(1), -vector_A_to_airplane_unit(0)); - } - - update_roll_setpoint(); -} - -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, - const Vector2f &ground_speed_vector) -{ - _has_guidance_updated = true; - - /* the complete guidance logic in this section was proposed by [2] */ - - /* - * 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); - - float eta = wrap_pi(_target_bearing - wrap_pi(current_heading)); - - /* 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; - - update_roll_setpoint(); -} - -void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) -{ - _has_guidance_updated = true; - - /* the logic in this section is trivial, but originally proposed by [2] */ - - /* 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; - - update_roll_setpoint(); } void ECL_L1_Pos_Controller::set_l1_period(float period) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp index e6e7464859..0b29d8cb38 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp @@ -83,14 +83,6 @@ public: */ float nav_lateral_acceleration_demand() { return _lateral_accel; } - /** - * Heading error. - * - * The heading error is either compared to the current track - * or to the tangent of the current loiter radius. - */ - float bearing_error() { return _bearing_error; } - /** * Bearing from aircraft to current target. * @@ -98,13 +90,6 @@ public: */ float target_bearing() { return _target_bearing; } - /** - * Get roll angle setpoint for fixed wing. - * - * @return Roll angle (in NED frame) - */ - float get_roll_setpoint() { return _roll_setpoint; } - /** * Get the current crosstrack error. * @@ -112,16 +97,6 @@ public: */ float crosstrack_error() { return _crosstrack_error; } - /** - * Returns true if the loiter waypoint has been reached - */ - bool reached_loiter_target() { return _circle_mode; } - - /** - * Returns true if following a circle (loiter) - */ - bool circle_mode() { return _circle_mode; } - /** * Get the switch distance * @@ -145,35 +120,6 @@ public: */ void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed); - /** - * 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 matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius, - const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector); - - /** - * Navigate on a fixed bearing. - * - * This only holds a certain direction and does not perform cross - * track correction. Helpful for semi-autonomous modes. Introduced - * by [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed); - - /** - * 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); /** * Set the L1 period. @@ -187,32 +133,11 @@ public: */ void set_l1_damping(float damping); - /** - * Set the maximum roll angle output in radians - */ - void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; } - - /** - * Set roll angle slew rate. Set to zero to deactivate. - */ - void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; } - - /** - * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting. - */ - void set_dt(float dt) { _dt = dt;} - - void reset_has_guidance_updated() { _has_guidance_updated = false; } - - bool has_guidance_updated() { return _has_guidance_updated; } - private: float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2 float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping - bool _circle_mode{false}; ///< flag for loiter mode float _nav_bearing{0.0f}; ///< bearing to L1 reference point - float _bearing_error{0.0f}; ///< bearing error float _crosstrack_error{0.0f}; ///< crosstrack error in meters float _target_bearing{0.0f}; ///< the heading setpoint @@ -221,21 +146,6 @@ private: float _L1_ratio{5.0f}; ///< L1 ratio for navigation float _K_L1{2.0f}; ///< L1 control gain for _L1_damping float _heading_omega{1.0f}; ///< Normalized frequency - - float _roll_lim_rad{math::radians(30.0f)}; ///