mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
tweaked Alt hold PIDs, loiter PIDs based on SIM testing and feedback.
Increased distance error for loiter for faster return to center.
This commit is contained in:
parent
97670246ff
commit
9f50cd14d1
@ -441,7 +441,7 @@
|
||||
// Navigation control gains
|
||||
//
|
||||
#ifndef LOITER_P
|
||||
# define LOITER_P .5 //
|
||||
# define LOITER_P .3 //
|
||||
#endif
|
||||
#ifndef LOITER_I
|
||||
# define LOITER_I 0.0 //
|
||||
@ -451,7 +451,7 @@
|
||||
#endif
|
||||
|
||||
#ifndef NAV_P
|
||||
# define NAV_P 4.0 //
|
||||
# define NAV_P 3.0 //
|
||||
#endif
|
||||
#ifndef NAV_I
|
||||
# define NAV_I 0.25 // this feels really low, 4s to move 1 degree pitch...
|
||||
@ -473,15 +473,16 @@
|
||||
#endif
|
||||
|
||||
#ifndef THR_HOLD_P
|
||||
# define THR_HOLD_P 0.80 //
|
||||
# define THR_HOLD_P 0.5 //
|
||||
#endif
|
||||
#ifndef THR_HOLD_I
|
||||
# define THR_HOLD_I 0.00 // with 4m error, 12.5s windup
|
||||
# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup
|
||||
#endif
|
||||
#ifndef THR_HOLD_IMAX
|
||||
# define THR_HOLD_IMAX 00
|
||||
# define THR_HOLD_IMAX 300
|
||||
#endif
|
||||
|
||||
// RATE control
|
||||
#ifndef THROTTLE_P
|
||||
# define THROTTLE_P 0.6 //
|
||||
#endif
|
||||
@ -489,7 +490,7 @@
|
||||
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
||||
#endif
|
||||
#ifndef THROTTLE_IMAX
|
||||
# define THROTTLE_IMAX 300
|
||||
# define THROTTLE_IMAX 50
|
||||
#endif
|
||||
|
||||
|
||||
|
@ -55,7 +55,7 @@ static void calc_location_error(struct Location *next_loc)
|
||||
lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
|
||||
}
|
||||
|
||||
#define NAV_ERR_MAX 400
|
||||
#define NAV_ERR_MAX 800
|
||||
static void calc_loiter(int x_error, int y_error)
|
||||
{
|
||||
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
|
Loading…
Reference in New Issue
Block a user