fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 23:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

tecs and l1 switch to matrix math library (#409)

Browse Source
This commit is contained in:
Daniel Agar
2018-03-18 21:59:41 -04:00
committed by GitHub
parent 1bc7378414
commit 35bc2cfcd9
4 changed files with 108 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
l1/ecl_l1_pos_controller.cpp
+70 -76
View File
@@ -42,6 +42,8 @@
#include "ecl_l1_pos_controller.h" #include "ecl_l1_pos_controller.h"
using matrix::Vector2f;
float ECL_L1_Pos_Controller::nav_roll() float ECL_L1_Pos_Controller::nav_roll()
{ {
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
@@ -49,54 +51,24 @@ float ECL_L1_Pos_Controller::nav_roll()
return ret; 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) float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{ {
/* following [2], switching on L1 distance */ /* following [2], switching on L1 distance */
return math::min(wp_radius, _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] */ /* this follows the logic presented in [1] */
float eta = 0.0f;
float eta; float xtrack_vel = 0.0f;
float xtrack_vel; float ltrack_vel = 0.0f;
float ltrack_vel;
/* get the direction between the last (visited) and next waypoint */ /* 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 */ /* enforce a minimum ground speed 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);
@@ -105,7 +77,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
_L1_distance = _L1_ratio * ground_speed; _L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */ /* 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, * 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(); vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */ /* 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) */ /* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane; _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; float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */ /* 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) */ /* calculate angle of airplane position vector relative to line) */
// XXX this could probably also be based solely on the dot product // 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); 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 */ /* 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 */ /* calculate eta to fly to waypoint A */
/* unit vector from waypoint A to current position */ /* 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 */ /* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
/* velocity along line */ /* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel); eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */ /* 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)) { } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
/* /*
* Extension, fly back to waypoint. * Extension, fly back to waypoint.
* *
* This corner case is possible if the system was following * This corner case is possible if the system was following
* the AB line from waypoint A to waypoint B, then is * the AB line from waypoint A to waypoint B, then is
* switched to manual mode (or otherwise misses the waypoint) * 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 */ /* calculate eta to fly to waypoint B */
/* velocity across / orthogonal to line */ /* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
/* velocity along line */ /* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
eta = atan2f(xtrack_vel, ltrack_vel); eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */ /* 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 { } 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); float eta2 = atan2f(xtrack_vel, ltrack_vel);
/* calculate eta1 (angle to L1 point) */ /* calculate eta1 (angle to L1 point) */
float xtrackErr = vector_A_to_airplane % vector_AB; 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 */ /* limit output to 45 degrees */
sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
float eta1 = asinf(sine_eta1); 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; _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, void
const math::Vector<2> &ground_speed_vector) 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] */ /* 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; float K_velocity = 2.0f * _L1_damping * omega;
/* update bearing to next waypoint */ /* 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 */ /* 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 */ /* calculate the L1 length required for the desired period */
_L1_distance = _L1_ratio * ground_speed; _L1_distance = _L1_ratio * ground_speed;
/* calculate the vector from waypoint A to current position */ /* 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 */ /* prevent NaN when normalizing */
if (vector_A_to_airplane.length() > FLT_EPSILON) { if (vector_A_to_airplane.length() > FLT_EPSILON) {
/* store the normalized vector from waypoint A to current position */ /* store the normalized vector from waypoint A to current position */
vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
} else { } else {
vector_A_to_airplane_unit = vector_A_to_airplane; 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 */ /* prevent PD output from turning the wrong way */
if (tangent_vel < 0.0f) { 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 */ /* 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 */ /* 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); 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 // XXX check switch over
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || 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; _lateral_accel = lateral_accel_sp_center;
_circle_mode = false; _circle_mode = false;
/* angle between requested and current velocity vector */ /* angle between requested and current velocity vector */
_bearing_error = eta; _bearing_error = eta;
/* bearing from current position to L1 point */ /* 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 { } else {
_lateral_accel = lateral_accel_sp_circle; _lateral_accel = lateral_accel_sp_circle;
_circle_mode = true; _circle_mode = true;
_bearing_error = 0.0f; _bearing_error = 0.0f;
/* bearing from current position to L1 point */ /* 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,
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) const Vector2f &ground_speed_vector)
{ {
/* the complete guidance logic in this section was proposed by [2] */ /* the complete guidance logic in this section was proposed by [2] */
float eta; /*
/*
* As the commanded heading is the only reference * As the commanded heading is the only reference
* (and no crosstrack correction occurs), * (and no crosstrack correction occurs),
* target and navigation bearing become the same * target and navigation bearing become the same
*/ */
_target_bearing = _nav_bearing = _wrap_pi(navigation_heading); _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); eta = _wrap_pi(eta);
/* consequently the bearing error is exactly eta: */ /* consequently the bearing error is exactly eta: */
_bearing_error = eta; _bearing_error = eta;
/* ground speed is the length of the ground speed vector */ /* ground speed is the length of the ground speed vector */
float ground_speed = ground_speed_vector.length(); 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 */ /* not circling a waypoint when flying level */
_circle_mode = false; _circle_mode = false;
} }
Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const
math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
{ {
/* this is an approximation for small angles, proposed by [2] */ /* this is an approximation for small angles, proposed by [2] */
Vector2f out(math::radians((target(0) - origin(0))),
math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH); return out * static_cast<float>(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;
}
l1/ecl_l1_pos_controller.h
+30 -79
View File
@@ -70,38 +70,19 @@
class __EXPORT ECL_L1_Pos_Controller class __EXPORT ECL_L1_Pos_Controller
{ {
public: 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 * The current target bearing
* *
* @return bearing angle (-pi..pi, in NED frame) * @return bearing angle (-pi..pi, in NED frame)
*/ */
float nav_bearing(); float nav_bearing() { return _wrap_pi(_nav_bearing); }
/** /**
* Get lateral acceleration demand. * Get lateral acceleration demand.
* *
* @return Lateral acceleration in m/s^2 * @return Lateral acceleration in m/s^2
*/ */
float nav_lateral_acceleration_demand(); float nav_lateral_acceleration_demand() { return _lateral_accel; }
/** /**
* Heading error. * Heading error.
@@ -109,16 +90,14 @@ public:
* The heading error is either compared to the current track * The heading error is either compared to the current track
* or to the tangent of the current loiter radius. * or to the tangent of the current loiter radius.
*/ */
float bearing_error(); float bearing_error() { return _bearing_error; }
/** /**
* Bearing from aircraft to current target. * Bearing from aircraft to current target.
* *
* @return bearing angle (-pi..pi, in NED frame) * @return bearing angle (-pi..pi, in NED frame)
*/ */
float target_bearing(); float target_bearing() { return _target_bearing; }
/** /**
* Get roll angle setpoint for fixed wing. * Get roll angle setpoint for fixed wing.
@@ -127,32 +106,26 @@ public:
*/ */
float nav_roll(); float nav_roll();
/** /**
* Get the current crosstrack error. * Get the current crosstrack error.
* *
* @return Crosstrack error in meters. * @return Crosstrack error in meters.
*/ */
float crosstrack_error(); float crosstrack_error() { return _crosstrack_error; }
/** /**
* Returns true if the loiter waypoint has been reached * 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) * Returns true if following a circle (loiter)
*/ */
bool circle_mode() { bool circle_mode() { return _circle_mode; }
return _circle_mode;
}
/** /**
* Get the switch distance * Get the switch distance
* *
* This is the distance at which the system will * This is the distance at which the system will
* switch to the next waypoint. This depends on the * switch to the next waypoint. This depends on the
* period and damping * period and damping
@@ -161,7 +134,6 @@ public:
*/ */
float switch_distance(float waypoint_switch_radius); float switch_distance(float waypoint_switch_radius);
/** /**
* Navigate between two waypoints * Navigate between two waypoints
* *
@@ -172,9 +144,8 @@ public:
* *
* @return sets _lateral_accel setpoint * @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, void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
const math::Vector<2> &ground_speed); const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
/** /**
* Navigate on an orbit around a loiter waypoint. * Navigate on an orbit around a loiter waypoint.
@@ -184,9 +155,8 @@ public:
* *
* @return sets _lateral_accel setpoint * @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, void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
const math::Vector<2> &ground_speed_vector); int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
/** /**
* Navigate on a fixed bearing. * Navigate on a fixed bearing.
@@ -197,8 +167,7 @@ public:
* *
* @return sets _lateral_accel setpoint * @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. * Keep the wings level.
@@ -208,58 +177,40 @@ public:
*/ */
void navigate_level_flight(float current_heading); void navigate_level_flight(float current_heading);
/** /**
* Set the L1 period. * Set the L1 period.
*/ */
void set_l1_period(float 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;
}
/** /**
* Set the L1 damping factor. * Set the L1 damping factor.
* *
* The original publication recommends a default of sqrt(2) / 2 = 0.707 * The original publication recommends a default of sqrt(2) / 2 = 0.707
*/ */
void set_l1_damping(float damping) { 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;
}
/** /**
* Set the maximum roll angle output in radians * Set the maximum roll angle output in radians
*
*/ */
void set_l1_roll_limit(float roll_lim_rad) { void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
_roll_lim_rad = roll_lim_rad;
}
private: private:
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2
float _L1_distance; ///< L1 lead distance, defined by period and damping float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping
bool _circle_mode; ///< flag for loiter mode bool _circle_mode{false}; ///< flag for loiter mode
float _nav_bearing; ///< bearing to L1 reference point float _nav_bearing{0.0f}; ///< bearing to L1 reference point
float _bearing_error; ///< bearing error float _bearing_error{0.0f}; ///< bearing error
float _crosstrack_error; ///< crosstrack error in meters float _crosstrack_error{0.0f}; ///< crosstrack error in meters
float _target_bearing; ///< the heading setpoint float _target_bearing{0.0f}; ///< the heading setpoint
float _L1_period; ///< L1 tracking period in seconds float _L1_period{25.0f}; ///< L1 tracking period in seconds
float _L1_damping; ///< L1 damping ratio float _L1_damping{0.75f}; ///< L1 damping ratio
float _L1_ratio; ///< L1 ratio for navigation float _L1_ratio{5.0f}; ///< L1 ratio for navigation
float _K_L1; ///< L1 control gain for _L1_damping float _K_L1{2.0f}; ///< L1 control gain for _L1_damping
float _heading_omega; ///< Normalized frequency float _heading_omega{1.0f}; ///< Normalized frequency
float _roll_lim_rad; ///<maximum roll angle float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle
/** /**
* Convert a 2D vector from WGS84 to planar coordinates. * Convert a 2D vector from WGS84 to planar coordinates.
@@ -272,7 +223,7 @@ private:
* @param wp The point to convert to into the local coordinates, 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 * @return The vector in meters pointing from the reference position to the coordinates
*/ */
math::Vector<2> 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;
}; };
tecs/tecs.cpp
+4 -4
View File
@@ -51,8 +51,8 @@ using math::min;
* inertial nav data is not available. It also calculates a true airspeed derivative * inertial nav data is not available. It also calculates a true airspeed derivative
* which is used by the airspeed complimentary filter. * which is used by the airspeed complimentary filter.
*/ */
void TECS::update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat, void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
const math::Vector<3> &accel_body, bool altitude_lock, bool in_air, const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
float altitude, bool vz_valid, float vz, float az) 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 _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 // Calculate total energy error
_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate; _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; _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 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) float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
{ {
tecs/tecs.h
+4 -4
View File
@@ -72,14 +72,14 @@ public:
* Must be called prior to udating tecs control loops * Must be called prior to udating tecs control loops
* Must be called at 50Hz or greater * Must be called at 50Hz or greater
*/ */
void update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat, void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
const math::Vector<3> &accel_body, bool altitude_lock, bool in_air, const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
float altitude, bool vz_valid, float vz, float az); float altitude, bool vz_valid, float vz, float az);
/** /**
* Update the control loop calculations * 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 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 throttle_min, float throttle_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max); float pitch_limit_min, float pitch_limit_max);
@@ -306,7 +306,7 @@ private:
/** /**
* Update throttle setpoint * 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 * Detect an uncommanded descent