aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

L1_Control: cleanup some unused code and variables

Browse Source
This commit is contained in:
Andrew Tridgell 2013-04-15 10:48:56 +10:00
parent a423d102e0
commit 571c48b9d5
2 changed files with 6 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -81,10 +81,10 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period
// Calculate additional damping gain
const float Kv = omegaA * 2.8284f * (_L1_damping - 0.7071f); // omegaA * 2*sqrt(2) * (dampingRatio - 1/sqrt(2))
float Nu;
float dampingWeight;
float xtrackVel;
struct Location _current_loc;
// Get current position and velocity
_ahrs->get_position(&_current_loc);
@ -178,6 +178,7 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
float omega = (6.2832f / _L1_period);
float Kx = omega * omega;
float Kv = 2.0f * _L1_damping * omega;
struct Location _current_loc;
//Get current position and velocity
_ahrs->get_position(&_current_loc);
@ -308,7 +309,7 @@ void AP_L1_Control::update_level_flight(void)
}
Vector2f AP_L1_Control::_geo2planar(const Vector2f &ref, const Vector2f &wp)
Vector2f AP_L1_Control::_geo2planar(const Vector2f &ref, const Vector2f &wp) const
{
Vector2f out;
@ -327,17 +328,7 @@ float AP_L1_Control::_cross2D(const Vector2f &v1, const Vector2f &v2)
return out;
}
Vector2f AP_L1_Control::_planar2geo(const Vector2f &ref, const Vector2f &wp)
{
Vector2f out;
out.x=degrees(wp.x)+ref.x;
out.y=degrees(wp.y*(1/cosf(radians(ref.x))))+ref.y;
return out;
}
float AP_L1_Control::_maxf(const float &num1, const float &num2)
float AP_L1_Control::_maxf(const float &num1, const float &num2) const
{
float result;

15
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -56,8 +56,6 @@ private:
// reference to the AHRS object
AP_AHRS *_ahrs;
struct Location _current_loc;
// lateral acceration in m/s required to fly to the
// L1 reference point (+ve to right)
float _latAccDem;
@ -84,25 +82,16 @@ private:
AP_Float _L1_period;
// L1 tracking loop damping ratio
AP_Float _L1_damping;
// L1 control loop enable
AP_Int8 _enable;
// Convert a 2D vector from latitude and longitude to planar
// coordinates based on a reference point
Vector2f _geo2planar(const Vector2f &ref, const Vector2f &wp);
Vector2f _geo2planar(const Vector2f &ref, const Vector2f &wp) const;
//Calculate the cross product of two 2D vectors
float _cross2D(const Vector2f &v1, const Vector2f &v2);
//Calculate the dot product of two 2D vectors
float _dot2D(const Vector2f &v1, const Vector2f &v2);
// Convert a 2D vector from planar coordinates to latitude and
// longitude based on a reference point
Vector2f _planar2geo(const Vector2f &ref, const Vector2f &wp);
//Calculate the maximum of two floating point numbers
float _maxf(const float &num1, const float &num2);
float _maxf(const float &num1, const float &num2) const;
};