mirror of https://github.com/ArduPilot/ardupilot
AP_L1_Control: remove tabs and trailing whitespaces
This commit is contained in:
Lucas De Marchi
parent
1dbffef7ea
commit
6f87195eb7
|
|
@ -10,16 +10,16 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
|
|||
// @Param: PERIOD
|
||||
// @DisplayName: L1 control period
|
||||
// @Description: Period in seconds of L1 tracking loop. This parameter is the primary control for agressiveness of turns in auto mode. This needs to be larger for less responsive airframes. The default of 20 is quite conservative, but for most RC aircraft will lead to reasonable flight. For smaller more agile aircraft a value closer to 15 is appropriate, or even as low as 10 for some very agile aircraft. When tuning, change this value in small increments, as a value that is much too small (say 5 or 10 below the right value) can lead to very radical turns, and a risk of stalling.
|
||||
// @Units: seconds
|
||||
// @Range: 1 60
|
||||
// @Increment: 1
|
||||
// @Units: seconds
|
||||
// @Range: 1 60
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PERIOD", 0, AP_L1_Control, _L1_period, 20),
|
||||
|
||||
|
||||
// @Param: DAMPING
|
||||
// @DisplayName: L1 control damping ratio
|
||||
// @Description: Damping ratio for L1 control. Increase this in increments of 0.05 if you are getting overshoot in path tracking. You should not need a value below 0.7 or above 0.85.
|
||||
// @Range: 0.6 1.0
|
||||
// @Increment: 0.05
|
||||
// @Range: 0.6 1.0
|
||||
// @Increment: 0.05
|
||||
AP_GROUPINFO("DAMPING", 1, AP_L1_Control, _L1_damping, 0.75f),
|
||||
|
||||
// @Param: XTRACK_I
|
||||
|
|
@ -33,7 +33,7 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
|
|||
};
|
||||
|
||||
//Bank angle command based on angle between aircraft velocity vector and reference vector to path.
|
||||
//S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
|
||||
//S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
|
||||
//Proceedings of the AIAA Guidance, Navigation and Control
|
||||
//Conference, Aug 2004. AIAA-2004-4900.
|
||||
//Modified to use PD control for circle tracking to enable loiter radius less than L1 length
|
||||
|
|
@ -47,10 +47,10 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
|
|||
*/
|
||||
int32_t AP_L1_Control::nav_roll_cd(void) const
|
||||
{
|
||||
float ret;
|
||||
ret = cosf(_ahrs.pitch)*degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81
|
||||
ret = constrain_float(ret, -9000, 9000);
|
||||
return ret;
|
||||
float ret;
|
||||
ret = cosf(_ahrs.pitch)*degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81
|
||||
ret = constrain_float(ret, -9000, 9000);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -59,22 +59,22 @@ int32_t AP_L1_Control::nav_roll_cd(void) const
|
|||
*/
|
||||
float AP_L1_Control::lateral_acceleration(void) const
|
||||
{
|
||||
return _latAccDem;
|
||||
return _latAccDem;
|
||||
}
|
||||
|
||||
int32_t AP_L1_Control::nav_bearing_cd(void) const
|
||||
{
|
||||
return wrap_180_cd(RadiansToCentiDegrees(_nav_bearing));
|
||||
return wrap_180_cd(RadiansToCentiDegrees(_nav_bearing));
|
||||
}
|
||||
|
||||
int32_t AP_L1_Control::bearing_error_cd(void) const
|
||||
{
|
||||
return RadiansToCentiDegrees(_bearing_error);
|
||||
return RadiansToCentiDegrees(_bearing_error);
|
||||
}
|
||||
|
||||
int32_t AP_L1_Control::target_bearing_cd(void) const
|
||||
{
|
||||
return wrap_180_cd(_target_bearing_cd);
|
||||
return wrap_180_cd(_target_bearing_cd);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -83,13 +83,13 @@ int32_t AP_L1_Control::target_bearing_cd(void) const
|
|||
float AP_L1_Control::turn_distance(float wp_radius) const
|
||||
{
|
||||
wp_radius *= sq(_ahrs.get_EAS2TAS());
|
||||
return MIN(wp_radius, _L1_dist);
|
||||
return MIN(wp_radius, _L1_dist);
|
||||
}
|
||||
|
||||
/*
|
||||
this approximates the turn distance for a given turn angle. If the
|
||||
turn_angle is > 90 then a 90 degree turn distance is used, otherwise
|
||||
the turn distance is reduced linearly.
|
||||
the turn distance is reduced linearly.
|
||||
This function allows straight ahead mission legs to avoid thinking
|
||||
they have reached the waypoint early, which makes things like camera
|
||||
trigger and ball drop at exact positions under mission control much easier
|
||||
|
|
@ -106,7 +106,7 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const
|
|||
|
||||
bool AP_L1_Control::reached_loiter_target(void)
|
||||
{
|
||||
return _WPcircle;
|
||||
return _WPcircle;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -133,10 +133,10 @@ void AP_L1_Control::_prevent_indecision(float &Nu)
|
|||
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP)
|
||||
{
|
||||
|
||||
struct Location _current_loc;
|
||||
float Nu;
|
||||
float xtrackVel;
|
||||
float ltrackVel;
|
||||
struct Location _current_loc;
|
||||
float Nu;
|
||||
float xtrackVel;
|
||||
float ltrackVel;
|
||||
|
||||
uint32_t now = AP_HAL::micros();
|
||||
float dt = (now - _last_update_waypoint_us) * 1.0e-6f;
|
||||
|
|
@ -144,20 +144,20 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
|
|||
dt = 0.1;
|
||||
}
|
||||
_last_update_waypoint_us = now;
|
||||
|
||||
// Calculate L1 gain required for specified damping
|
||||
float K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
|
||||
// Get current position and velocity
|
||||
// Calculate L1 gain required for specified damping
|
||||
float K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
|
||||
// Get current position and velocity
|
||||
_ahrs.get_position(_current_loc);
|
||||
|
||||
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
|
||||
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
|
||||
|
||||
// update _target_bearing_cd
|
||||
_target_bearing_cd = get_bearing_cd(_current_loc, next_WP);
|
||||
|
||||
//Calculate groundspeed
|
||||
float groundSpeed = _groundspeed_vector.length();
|
||||
// update _target_bearing_cd
|
||||
_target_bearing_cd = get_bearing_cd(_current_loc, next_WP);
|
||||
|
||||
//Calculate groundspeed
|
||||
float groundSpeed = _groundspeed_vector.length();
|
||||
if (groundSpeed < 0.1f) {
|
||||
// use a small ground speed vector in the right direction,
|
||||
// allowing us to use the compass heading at zero GPS velocity
|
||||
|
|
@ -165,136 +165,136 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
|
|||
_groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed;
|
||||
}
|
||||
|
||||
// Calculate time varying control parameters
|
||||
// Calculate the L1 length required for specified period
|
||||
// 0.3183099 = 1/1/pipi
|
||||
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
|
||||
|
||||
// Calculate the NE position of WP B relative to WP A
|
||||
// Calculate time varying control parameters
|
||||
// Calculate the L1 length required for specified period
|
||||
// 0.3183099 = 1/1/pipi
|
||||
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
|
||||
|
||||
// Calculate the NE position of WP B relative to WP A
|
||||
Vector2f AB = location_diff(prev_WP, next_WP);
|
||||
float AB_length = AB.length();
|
||||
|
||||
// Check for AB zero length and track directly to the destination
|
||||
// if too small
|
||||
if (AB.length() < 1.0e-6f) {
|
||||
AB = location_diff(_current_loc, next_WP);
|
||||
|
||||
// Check for AB zero length and track directly to the destination
|
||||
// if too small
|
||||
if (AB.length() < 1.0e-6f) {
|
||||
AB = location_diff(_current_loc, next_WP);
|
||||
if (AB.length() < 1.0e-6f) {
|
||||
AB = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw));
|
||||
}
|
||||
}
|
||||
AB.normalize();
|
||||
}
|
||||
AB.normalize();
|
||||
|
||||
// Calculate the NE position of the aircraft relative to WP A
|
||||
// Calculate the NE position of the aircraft relative to WP A
|
||||
Vector2f A_air = location_diff(prev_WP, _current_loc);
|
||||
|
||||
// calculate distance to target track, for reporting
|
||||
_crosstrack_error = A_air % AB;
|
||||
// calculate distance to target track, for reporting
|
||||
_crosstrack_error = A_air % AB;
|
||||
|
||||
//Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
|
||||
//and further than L1 distance from WP A. Then use WP A as the L1 reference point
|
||||
//Otherwise do normal L1 guidance
|
||||
float WP_A_dist = A_air.length();
|
||||
float alongTrackDist = A_air * AB;
|
||||
if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)
|
||||
//Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
|
||||
//and further than L1 distance from WP A. Then use WP A as the L1 reference point
|
||||
//Otherwise do normal L1 guidance
|
||||
float WP_A_dist = A_air.length();
|
||||
float alongTrackDist = A_air * AB;
|
||||
if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)
|
||||
{
|
||||
//Calc Nu to fly To WP A
|
||||
Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
|
||||
xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
|
||||
ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
|
||||
Nu = atan2f(xtrackVel,ltrackVel);
|
||||
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
|
||||
//Calc Nu to fly To WP A
|
||||
Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
|
||||
xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
|
||||
ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
|
||||
Nu = atan2f(xtrackVel,ltrackVel);
|
||||
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
|
||||
} else if (alongTrackDist > AB_length + groundSpeed*3) {
|
||||
// we have passed point B by 3 seconds. Head towards B
|
||||
// Calc Nu to fly To WP B
|
||||
Vector2f B_air = location_diff(next_WP, _current_loc);
|
||||
Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft
|
||||
xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line
|
||||
ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line
|
||||
Nu = atan2f(xtrackVel,ltrackVel);
|
||||
_nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point
|
||||
} else { //Calc Nu to fly along AB line
|
||||
|
||||
//Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
|
||||
xtrackVel = _groundspeed_vector % AB; // Velocity cross track
|
||||
ltrackVel = _groundspeed_vector * AB; // Velocity along track
|
||||
float Nu2 = atan2f(xtrackVel,ltrackVel);
|
||||
//Calculate Nu1 angle (Angle to L1 reference point)
|
||||
float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f);
|
||||
//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
|
||||
sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
|
||||
float Nu1 = asinf(sine_Nu1);
|
||||
Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft
|
||||
xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line
|
||||
ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line
|
||||
Nu = atan2f(xtrackVel,ltrackVel);
|
||||
_nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point
|
||||
} else { //Calc Nu to fly along AB line
|
||||
|
||||
//Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
|
||||
xtrackVel = _groundspeed_vector % AB; // Velocity cross track
|
||||
ltrackVel = _groundspeed_vector * AB; // Velocity along track
|
||||
float Nu2 = atan2f(xtrackVel,ltrackVel);
|
||||
//Calculate Nu1 angle (Angle to L1 reference point)
|
||||
float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f);
|
||||
//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
|
||||
sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
|
||||
float Nu1 = asinf(sine_Nu1);
|
||||
|
||||
// compute integral error component to converge to a crosstrack of zero when traveling
|
||||
// straight but reset it when disabled or if it changes. That allows for much easier
|
||||
// tuning by having it re-converge each time it changes.
|
||||
// straight but reset it when disabled or if it changes. That allows for much easier
|
||||
// tuning by having it re-converge each time it changes.
|
||||
if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
|
||||
_L1_xtrack_i = 0;
|
||||
_L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
|
||||
} else if (fabsf(Nu1) < radians(5)) {
|
||||
_L1_xtrack_i = 0;
|
||||
_L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
|
||||
} else if (fabsf(Nu1) < radians(5)) {
|
||||
_L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt;
|
||||
|
||||
// an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at
|
||||
_L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f);
|
||||
}
|
||||
}
|
||||
|
||||
// to converge to zero we must push Nu1 harder
|
||||
// to converge to zero we must push Nu1 harder
|
||||
Nu1 += _L1_xtrack_i;
|
||||
|
||||
Nu = Nu1 + Nu2;
|
||||
_nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point
|
||||
}
|
||||
Nu = Nu1 + Nu2;
|
||||
_nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point
|
||||
}
|
||||
|
||||
_prevent_indecision(Nu);
|
||||
_last_Nu = Nu;
|
||||
|
||||
//Limit Nu to +-pi
|
||||
Nu = constrain_float(Nu, -1.5708f, +1.5708f);
|
||||
_latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
|
||||
|
||||
// Waypoint capture status is always false during waypoint following
|
||||
_WPcircle = false;
|
||||
|
||||
_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
|
||||
|
||||
_data_is_stale = false; // status are correctly updated with current waypoint data
|
||||
//Limit Nu to +-pi
|
||||
Nu = constrain_float(Nu, -1.5708f, +1.5708f);
|
||||
_latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
|
||||
|
||||
// Waypoint capture status is always false during waypoint following
|
||||
_WPcircle = false;
|
||||
|
||||
_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
|
||||
|
||||
_data_is_stale = false; // status are correctly updated with current waypoint data
|
||||
}
|
||||
|
||||
// update L1 control for loitering
|
||||
void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction)
|
||||
{
|
||||
struct Location _current_loc;
|
||||
struct Location _current_loc;
|
||||
|
||||
// scale loiter radius with square of EAS2TAS to allow us to stay
|
||||
// stable at high altitude
|
||||
radius *= sq(_ahrs.get_EAS2TAS());
|
||||
|
||||
// Calculate guidance gains used by PD loop (used during circle tracking)
|
||||
float omega = (6.2832f / _L1_period);
|
||||
float Kx = omega * omega;
|
||||
float Kv = 2.0f * _L1_damping * omega;
|
||||
// Calculate guidance gains used by PD loop (used during circle tracking)
|
||||
float omega = (6.2832f / _L1_period);
|
||||
float Kx = omega * omega;
|
||||
float Kv = 2.0f * _L1_damping * omega;
|
||||
|
||||
// Calculate L1 gain required for specified damping (used during waypoint capture)
|
||||
float K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
// Calculate L1 gain required for specified damping (used during waypoint capture)
|
||||
float K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
|
||||
//Get current position and velocity
|
||||
//Get current position and velocity
|
||||
_ahrs.get_position(_current_loc);
|
||||
|
||||
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
|
||||
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
|
||||
|
||||
//Calculate groundspeed
|
||||
float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f);
|
||||
//Calculate groundspeed
|
||||
float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f);
|
||||
|
||||
|
||||
// update _target_bearing_cd
|
||||
_target_bearing_cd = get_bearing_cd(_current_loc, center_WP);
|
||||
// update _target_bearing_cd
|
||||
_target_bearing_cd = get_bearing_cd(_current_loc, center_WP);
|
||||
|
||||
|
||||
// Calculate time varying control parameters
|
||||
// Calculate the L1 length required for specified period
|
||||
// 0.3183099 = 1/pi
|
||||
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
|
||||
// Calculate time varying control parameters
|
||||
// Calculate the L1 length required for specified period
|
||||
// 0.3183099 = 1/pi
|
||||
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
|
||||
|
||||
//Calculate the NE position of the aircraft relative to WP A
|
||||
//Calculate the NE position of the aircraft relative to WP A
|
||||
Vector2f A_air = location_diff(center_WP, _current_loc);
|
||||
|
||||
// Calculate the unit vector from WP A to aircraft
|
||||
|
|
@ -312,57 +312,57 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
|
|||
}
|
||||
}
|
||||
|
||||
//Calculate Nu to capture center_WP
|
||||
float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
|
||||
float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
|
||||
float Nu = atan2f(xtrackVelCap,ltrackVelCap);
|
||||
//Calculate Nu to capture center_WP
|
||||
float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
|
||||
float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
|
||||
float Nu = atan2f(xtrackVelCap,ltrackVelCap);
|
||||
|
||||
_prevent_indecision(Nu);
|
||||
_last_Nu = Nu;
|
||||
|
||||
Nu = constrain_float(Nu, -M_PI_2, M_PI_2); //Limit Nu to +- Pi/2
|
||||
Nu = constrain_float(Nu, -M_PI_2, M_PI_2); //Limit Nu to +- Pi/2
|
||||
|
||||
//Calculate lat accln demand to capture center_WP (use L1 guidance law)
|
||||
float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
|
||||
|
||||
//Calculate radial position and velocity errors
|
||||
float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
|
||||
float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle
|
||||
//Calculate lat accln demand to capture center_WP (use L1 guidance law)
|
||||
float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
|
||||
|
||||
//Calculate radial position and velocity errors
|
||||
float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
|
||||
float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle
|
||||
|
||||
// keep crosstrack error for reporting
|
||||
_crosstrack_error = xtrackErrCirc;
|
||||
|
||||
//Calculate PD control correction to circle waypoint_ahrs.roll
|
||||
float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
|
||||
|
||||
//Calculate tangential velocity
|
||||
float velTangent = xtrackVelCap * float(loiter_direction);
|
||||
|
||||
// keep crosstrack error for reporting
|
||||
_crosstrack_error = xtrackErrCirc;
|
||||
|
||||
//Calculate PD control correction to circle waypoint_ahrs.roll
|
||||
float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
|
||||
|
||||
//Calculate tangential velocity
|
||||
float velTangent = xtrackVelCap * float(loiter_direction);
|
||||
|
||||
//Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
|
||||
if (ltrackVelCap < 0.0f && velTangent < 0.0f) {
|
||||
latAccDemCircPD = MAX(latAccDemCircPD, 0.0f);
|
||||
}
|
||||
|
||||
// Calculate centripetal acceleration demand
|
||||
float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc));
|
||||
}
|
||||
|
||||
//Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
|
||||
float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
|
||||
|
||||
// Perform switchover between 'capture' and 'circle' modes at the
|
||||
// point where the commands cross over to achieve a seamless transfer
|
||||
// Only fly 'capture' mode if outside the circle
|
||||
if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
|
||||
_latAccDem = latAccDemCap;
|
||||
_WPcircle = false;
|
||||
_bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
|
||||
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
|
||||
} else {
|
||||
_latAccDem = latAccDemCirc;
|
||||
_WPcircle = true;
|
||||
_bearing_error = 0.0f; // bearing error (radians), +ve to left of track
|
||||
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
|
||||
}
|
||||
// Calculate centripetal acceleration demand
|
||||
float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc));
|
||||
|
||||
//Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
|
||||
float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
|
||||
|
||||
// Perform switchover between 'capture' and 'circle' modes at the
|
||||
// point where the commands cross over to achieve a seamless transfer
|
||||
// Only fly 'capture' mode if outside the circle
|
||||
if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
|
||||
_latAccDem = latAccDemCap;
|
||||
_WPcircle = false;
|
||||
_bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
|
||||
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
|
||||
} else {
|
||||
_latAccDem = latAccDemCirc;
|
||||
_WPcircle = true;
|
||||
_bearing_error = 0.0f; // bearing error (radians), +ve to left of track
|
||||
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
|
||||
}
|
||||
|
||||
_data_is_stale = false; // status are correctly updated with current waypoint data
|
||||
}
|
||||
|
|
@ -371,40 +371,40 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
|
|||
// update L1 control for heading hold navigation
|
||||
void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
|
||||
{
|
||||
// Calculate normalised frequency for tracking loop
|
||||
const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period
|
||||
// Calculate additional damping gain
|
||||
// Calculate normalised frequency for tracking loop
|
||||
const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period
|
||||
// Calculate additional damping gain
|
||||
|
||||
int32_t Nu_cd;
|
||||
float Nu;
|
||||
|
||||
// copy to _target_bearing_cd and _nav_bearing
|
||||
_target_bearing_cd = wrap_180_cd(navigation_heading_cd);
|
||||
_nav_bearing = radians(navigation_heading_cd * 0.01f);
|
||||
int32_t Nu_cd;
|
||||
float Nu;
|
||||
|
||||
Nu_cd = _target_bearing_cd - wrap_180_cd(_ahrs.yaw_sensor);
|
||||
Nu_cd = wrap_180_cd(Nu_cd);
|
||||
Nu = radians(Nu_cd * 0.01f);
|
||||
// copy to _target_bearing_cd and _nav_bearing
|
||||
_target_bearing_cd = wrap_180_cd(navigation_heading_cd);
|
||||
_nav_bearing = radians(navigation_heading_cd * 0.01f);
|
||||
|
||||
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
|
||||
|
||||
//Calculate groundspeed
|
||||
float groundSpeed = _groundspeed_vector.length();
|
||||
Nu_cd = _target_bearing_cd - wrap_180_cd(_ahrs.yaw_sensor);
|
||||
Nu_cd = wrap_180_cd(Nu_cd);
|
||||
Nu = radians(Nu_cd * 0.01f);
|
||||
|
||||
// Calculate time varying control parameters
|
||||
_L1_dist = groundSpeed / omegaA; // L1 distance is adjusted to maintain a constant tracking loop frequency
|
||||
float VomegaA = groundSpeed * omegaA;
|
||||
|
||||
// Waypoint capture status is always false during heading hold
|
||||
_WPcircle = false;
|
||||
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
|
||||
|
||||
_crosstrack_error = 0;
|
||||
//Calculate groundspeed
|
||||
float groundSpeed = _groundspeed_vector.length();
|
||||
|
||||
_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
|
||||
// Calculate time varying control parameters
|
||||
_L1_dist = groundSpeed / omegaA; // L1 distance is adjusted to maintain a constant tracking loop frequency
|
||||
float VomegaA = groundSpeed * omegaA;
|
||||
|
||||
// Limit Nu to +-pi
|
||||
Nu = constrain_float(Nu, -M_PI_2, M_PI_2);
|
||||
_latAccDem = 2.0f*sinf(Nu)*VomegaA;
|
||||
// Waypoint capture status is always false during heading hold
|
||||
_WPcircle = false;
|
||||
|
||||
_crosstrack_error = 0;
|
||||
|
||||
_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
|
||||
|
||||
// Limit Nu to +-pi
|
||||
Nu = constrain_float(Nu, -M_PI_2, M_PI_2);
|
||||
_latAccDem = 2.0f*sinf(Nu)*VomegaA;
|
||||
|
||||
_data_is_stale = false; // status are correctly updated with current waypoint data
|
||||
}
|
||||
|
|
@ -412,16 +412,16 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
|
|||
// update L1 control for level flight on current heading
|
||||
void AP_L1_Control::update_level_flight(void)
|
||||
{
|
||||
// copy to _target_bearing_cd and _nav_bearing
|
||||
_target_bearing_cd = _ahrs.yaw_sensor;
|
||||
_nav_bearing = _ahrs.yaw;
|
||||
_bearing_error = 0;
|
||||
_crosstrack_error = 0;
|
||||
// copy to _target_bearing_cd and _nav_bearing
|
||||
_target_bearing_cd = _ahrs.yaw_sensor;
|
||||
_nav_bearing = _ahrs.yaw;
|
||||
_bearing_error = 0;
|
||||
_crosstrack_error = 0;
|
||||
|
||||
// Waypoint capture status is always false during heading hold
|
||||
_WPcircle = false;
|
||||
// Waypoint capture status is always false during heading hold
|
||||
_WPcircle = false;
|
||||
|
||||
_latAccDem = 0;
|
||||
_latAccDem = 0;
|
||||
|
||||
_data_is_stale = false; // status are correctly updated with current waypoint data
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,34 +21,34 @@
|
|||
|
||||
class AP_L1_Control : public AP_Navigation {
|
||||
public:
|
||||
AP_L1_Control(AP_AHRS &ahrs) :
|
||||
_ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
AP_L1_Control(AP_AHRS &ahrs)
|
||||
: _ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
/* see AP_Navigation.h for the definitions and units of these
|
||||
* functions */
|
||||
int32_t nav_roll_cd(void) const;
|
||||
float lateral_acceleration(void) const;
|
||||
/* see AP_Navigation.h for the definitions and units of these
|
||||
* functions */
|
||||
int32_t nav_roll_cd(void) const;
|
||||
float lateral_acceleration(void) const;
|
||||
|
||||
// return the desired track heading angle(centi-degrees)
|
||||
int32_t nav_bearing_cd(void) const;
|
||||
|
||||
// return the heading error angle (centi-degrees) +ve to left of track
|
||||
int32_t bearing_error_cd(void) const;
|
||||
// return the desired track heading angle(centi-degrees)
|
||||
int32_t nav_bearing_cd(void) const;
|
||||
|
||||
// return the heading error angle (centi-degrees) +ve to left of track
|
||||
int32_t bearing_error_cd(void) const;
|
||||
|
||||
float crosstrack_error(void) const { return _crosstrack_error; }
|
||||
float crosstrack_error_integrator(void) const { return _L1_xtrack_i; }
|
||||
|
||||
int32_t target_bearing_cd(void) const;
|
||||
float turn_distance(float wp_radius) const;
|
||||
float turn_distance(float wp_radius, float turn_angle) const;
|
||||
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
|
||||
void update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction);
|
||||
void update_heading_hold(int32_t navigation_heading_cd);
|
||||
void update_level_flight(void);
|
||||
bool reached_loiter_target(void);
|
||||
int32_t target_bearing_cd(void) const;
|
||||
float turn_distance(float wp_radius) const;
|
||||
float turn_distance(float wp_radius, float turn_angle) const;
|
||||
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
|
||||
void update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction);
|
||||
void update_heading_hold(int32_t navigation_heading_cd);
|
||||
void update_level_flight(void);
|
||||
bool reached_loiter_target(void);
|
||||
|
||||
// set the default NAVL1_PERIOD
|
||||
void set_default_period(float period) {
|
||||
|
|
@ -62,39 +62,39 @@ public:
|
|||
return _data_is_stale;
|
||||
}
|
||||
|
||||
// this supports the NAVl1_* user settable parameters
|
||||
// this supports the NAVl1_* user settable parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
private:
|
||||
// reference to the AHRS object
|
||||
// reference to the AHRS object
|
||||
AP_AHRS &_ahrs;
|
||||
|
||||
// lateral acceration in m/s required to fly to the
|
||||
// L1 reference point (+ve to right)
|
||||
// lateral acceration in m/s required to fly to the
|
||||
// L1 reference point (+ve to right)
|
||||
float _latAccDem;
|
||||
|
||||
// L1 tracking distance in meters which is dynamically updated
|
||||
float _L1_dist;
|
||||
|
||||
// Status which is true when the vehicle has started circling the WP
|
||||
bool _WPcircle;
|
||||
|
||||
// bearing angle (radians) to L1 point
|
||||
float _nav_bearing;
|
||||
|
||||
// bearing error angle (radians) +ve to left of track
|
||||
float _bearing_error;
|
||||
|
||||
// crosstrack error in meters
|
||||
float _crosstrack_error;
|
||||
// L1 tracking distance in meters which is dynamically updated
|
||||
float _L1_dist;
|
||||
|
||||
// target bearing in centi-degrees from last update
|
||||
int32_t _target_bearing_cd;
|
||||
// Status which is true when the vehicle has started circling the WP
|
||||
bool _WPcircle;
|
||||
|
||||
// L1 tracking loop period (sec)
|
||||
AP_Float _L1_period;
|
||||
// L1 tracking loop damping ratio
|
||||
AP_Float _L1_damping;
|
||||
// bearing angle (radians) to L1 point
|
||||
float _nav_bearing;
|
||||
|
||||
// bearing error angle (radians) +ve to left of track
|
||||
float _bearing_error;
|
||||
|
||||
// crosstrack error in meters
|
||||
float _crosstrack_error;
|
||||
|
||||
// target bearing in centi-degrees from last update
|
||||
int32_t _target_bearing_cd;
|
||||
|
||||
// L1 tracking loop period (sec)
|
||||
AP_Float _L1_period;
|
||||
// L1 tracking loop damping ratio
|
||||
AP_Float _L1_damping;
|
||||
|
||||
// previous value of cross-track velocity
|
||||
float _last_Nu;
|
||||
|
|
|
|||
Loading…
Reference in New Issue