diff --git a/l1/ecl_l1_pos_controller.cpp b/l1/ecl_l1_pos_controller.cpp index 5f512e40cf..8ef168e365 100644 --- a/l1/ecl_l1_pos_controller.cpp +++ b/l1/ecl_l1_pos_controller.cpp @@ -42,6 +42,8 @@ #include "ecl_l1_pos_controller.h" +using matrix::Vector2f; + float ECL_L1_Pos_Controller::nav_roll() { float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); @@ -49,54 +51,24 @@ float ECL_L1_Pos_Controller::nav_roll() return ret; } -float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() -{ - return _lateral_accel; -} - -float ECL_L1_Pos_Controller::nav_bearing() -{ - return _wrap_pi(_nav_bearing); -} - -float ECL_L1_Pos_Controller::bearing_error() -{ - return _bearing_error; -} - -float ECL_L1_Pos_Controller::target_bearing() -{ - return _target_bearing; -} - float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ return math::min(wp_radius, _L1_distance); } -bool ECL_L1_Pos_Controller::reached_loiter_target() +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) { - return _circle_mode; -} - -float ECL_L1_Pos_Controller::crosstrack_error() -{ - return _crosstrack_error; -} - -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, - const math::Vector<2> &ground_speed_vector) -{ - /* this follows the logic presented in [1] */ - - float eta; - float xtrack_vel; - float ltrack_vel; + float eta = 0.0f; + float xtrack_vel = 0.0f; + float ltrack_vel = 0.0f; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), + vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -105,7 +77,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); + Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -118,7 +90,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + 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; @@ -132,36 +104,37 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - + Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + /* calculate angle of airplane position vector relative to line) */ // XXX this could probably also be based solely on the dot product float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); /* extension from [2], fly directly to A */ - if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { + 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::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + 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(1) , -vector_A_to_airplane_unit(0)); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); + + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ - /* - * If the AB vector and the vector from B to airplane point in the same - * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. - */ } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { /* * Extension, fly back to waypoint. - * + * * This corner case is possible if the system was following * the AB line from waypoint A to waypoint B, then is * switched to manual mode (or otherwise misses the waypoint) @@ -169,14 +142,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, */ /* calculate eta to fly to waypoint B */ - + /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); + _nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0)); } else { @@ -190,7 +163,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, 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); + float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f); /* limit output to 45 degrees */ sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 float eta1 = asinf(sine_eta1); @@ -211,8 +184,9 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector<2> &ground_speed_vector) +void +ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius, + int8_t loiter_direction, const Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -222,23 +196,25 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), + vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length() , 0.1f); + 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::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - math::Vector<2> vector_A_to_airplane_unit; + 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; } @@ -274,11 +250,12 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* 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); + 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)); + 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); @@ -290,41 +267,39 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con // 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_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(1) , -vector_A_to_airplane_unit(0)); + _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)); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); } } - -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, + const Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ - 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); + float eta = _target_bearing - _wrap_pi(current_heading); eta = _wrap_pi(eta); /* consequently the bearing error is exactly eta: */ - _bearing_error = eta; + _bearing_error = eta; /* ground speed is the length of the ground speed vector */ float ground_speed = ground_speed_vector.length(); @@ -357,16 +332,35 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) /* not circling a waypoint when flying level */ _circle_mode = false; - } - -math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const +Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const { /* this is an approximation for small angles, proposed by [2] */ - - math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); + Vector2f out(math::radians((target(0) - origin(0))), + math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); } +void ECL_L1_Pos_Controller::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; +} + +void ECL_L1_Pos_Controller::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; +} diff --git a/l1/ecl_l1_pos_controller.h b/l1/ecl_l1_pos_controller.h index 1e20e44e8e..3f28c31f8d 100644 --- a/l1/ecl_l1_pos_controller.h +++ b/l1/ecl_l1_pos_controller.h @@ -70,38 +70,19 @@ class __EXPORT ECL_L1_Pos_Controller { public: - ECL_L1_Pos_Controller() : - _lateral_accel(0.0), - _L1_distance(20.0), - _circle_mode(false), - _nav_bearing(0.0), - _bearing_error(0.0), - _crosstrack_error(0.0), - _target_bearing(0.0), - _L1_period(25.0), - _L1_damping(0.75), - _L1_ratio(5.0), - _K_L1(2.0), - _heading_omega(1.0), - _roll_lim_rad(math::radians(10.0)) - { - } - /** * The current target bearing * * @return bearing angle (-pi..pi, in NED frame) */ - float nav_bearing(); - + float nav_bearing() { return _wrap_pi(_nav_bearing); } /** * Get lateral acceleration demand. * * @return Lateral acceleration in m/s^2 */ - float nav_lateral_acceleration_demand(); - + float nav_lateral_acceleration_demand() { return _lateral_accel; } /** * Heading error. @@ -109,16 +90,14 @@ public: * The heading error is either compared to the current track * or to the tangent of the current loiter radius. */ - float bearing_error(); - + float bearing_error() { return _bearing_error; } /** * Bearing from aircraft to current target. * * @return bearing angle (-pi..pi, in NED frame) */ - float target_bearing(); - + float target_bearing() { return _target_bearing; } /** * Get roll angle setpoint for fixed wing. @@ -127,32 +106,26 @@ public: */ float nav_roll(); - /** * Get the current crosstrack error. * * @return Crosstrack error in meters. */ - float crosstrack_error(); - + float crosstrack_error() { return _crosstrack_error; } /** * Returns true if the loiter waypoint has been reached */ - bool reached_loiter_target(); - + bool reached_loiter_target() { return _circle_mode; } /** * Returns true if following a circle (loiter) */ - bool circle_mode() { - return _circle_mode; - } - + bool circle_mode() { return _circle_mode; } /** * 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 @@ -161,7 +134,6 @@ public: */ float switch_distance(float waypoint_switch_radius); - /** * Navigate between two waypoints * @@ -172,9 +144,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, - const math::Vector<2> &ground_speed); - + 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. @@ -184,9 +155,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector<2> &ground_speed_vector); - + void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius, + int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector); /** * Navigate on a fixed bearing. @@ -197,8 +167,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); - + void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed); /** * Keep the wings level. @@ -208,58 +177,40 @@ public: */ void navigate_level_flight(float current_heading); - /** * Set the L1 period. */ - 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; - } - + void set_l1_period(float 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; - /* 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; - } - + 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; - } + void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; } 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 _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 - 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 + float _L1_period{25.0f}; ///< L1 tracking period in seconds + float _L1_damping{0.75f}; ///< L1 damping ratio + 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; /// get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; + matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const; }; diff --git a/tecs/tecs.cpp b/tecs/tecs.cpp index 1cfdb41114..7680e863b3 100644 --- a/tecs/tecs.cpp +++ b/tecs/tecs.cpp @@ -51,8 +51,8 @@ using math::min; * inertial nav data is not available. It also calculates a true airspeed derivative * which is used by the airspeed complimentary filter. */ -void TECS::update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat, - const math::Vector<3> &accel_body, bool altitude_lock, bool in_air, +void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, + const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, float altitude, bool vz_valid, float vz, float az) { @@ -306,7 +306,7 @@ void TECS::_update_energy_estimates() _SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change } -void TECS::_update_throttle_setpoint(const float throttle_cruise, const math::Matrix<3, 3> &rotMat) +void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat) { // Calculate total energy error _STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate; @@ -575,7 +575,7 @@ void TECS::_update_STE_rate_lim() _STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_setpoint, +void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/tecs/tecs.h b/tecs/tecs.h index b13add44c6..1a0dec3ffe 100644 --- a/tecs/tecs.h +++ b/tecs/tecs.h @@ -72,14 +72,14 @@ public: * Must be called prior to udating tecs control loops * Must be called at 50Hz or greater */ - void update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat, - const math::Vector<3> &accel_body, bool altitude_lock, bool in_air, + void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, + const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, float altitude, bool vz_valid, float vz, float az); /** * Update the control loop calculations */ - void update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_setpoint, + void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); @@ -306,7 +306,7 @@ private: /** * Update throttle setpoint */ - void _update_throttle_setpoint(float throttle_cruise, const math::Matrix<3, 3> &rotMat); + void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat); /** * Detect an uncommanded descent