mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
Revert "test to switch loiter controls"
This reverts commit 92270371d038f246d535f0ace9fc19272c44291d.
This commit is contained in:
parent
9c1e257826
commit
74daadb252
@ -628,7 +628,7 @@
|
||||
# define LOITER_IMAX 30 // degrees°
|
||||
#endif
|
||||
#ifndef LOITER_D
|
||||
# define LOITER_D 3 // rate control
|
||||
# define LOITER_D 2.2 // rate control
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -143,13 +143,13 @@ static void calc_location_error(struct Location *next_loc)
|
||||
*/
|
||||
|
||||
#define NAV_ERR_MAX 800
|
||||
static void calc_loiter(int x_error, int y_error)
|
||||
static void calc_loiter1(int x_error, int y_error)
|
||||
{
|
||||
int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav);
|
||||
int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps
|
||||
int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps
|
||||
|
||||
int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav);
|
||||
int16_t lat_D = g.loiter_d * y_actual_speed ;
|
||||
int16_t lat_D = 3 * y_actual_speed ;
|
||||
|
||||
//limit of terms
|
||||
lon_D = constrain(lon_D,-500,500);
|
||||
@ -160,7 +160,7 @@ static void calc_loiter(int x_error, int y_error)
|
||||
}
|
||||
|
||||
|
||||
static void calc_loiter1(int x_error, int y_error)
|
||||
static void calc_loiter(int x_error, int y_error)
|
||||
{
|
||||
// East/West
|
||||
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800
|
||||
|
Loading…
Reference in New Issue
Block a user