fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 09:03:34 +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.h
+30 -79
View File
@@ -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; ///<maximum roll angle
float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle
/**
* 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
* @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;
};