Copter: remove OF_Loiter PIDs and tuning

This commit is contained in:
Randy Mackay 2014-12-06 17:13:03 +09:00
parent 0994529624
commit 67fdfffc2a
4 changed files with 5 additions and 88 deletions

View File

@ -579,18 +579,6 @@ static float baro_climbrate; // barometer climbrate in cm/s
// Current location of the copter
static struct Location current_loc;
////////////////////////////////////////////////////////////////////////////////
// Navigation Roll/Pitch functions
////////////////////////////////////////////////////////////////////////////////
#if OPTFLOW == ENABLED
// The Commanded ROll from the autopilot based on optical flow sensor.
static int32_t of_roll;
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
static int32_t of_pitch;
#endif // OPTFLOW == ENABLED
////////////////////////////////////////////////////////////////////////////////
// Throttle integrator
////////////////////////////////////////////////////////////////////////////////
@ -1520,21 +1508,6 @@ static void tuning(){
break;
#endif
case CH6_OPTFLOW_KP:
g.pid_optflow_roll.kP(tuning_value);
g.pid_optflow_pitch.kP(tuning_value);
break;
case CH6_OPTFLOW_KI:
g.pid_optflow_roll.kI(tuning_value);
g.pid_optflow_pitch.kI(tuning_value);
break;
case CH6_OPTFLOW_KD:
g.pid_optflow_roll.kD(tuning_value);
g.pid_optflow_pitch.kD(tuning_value);
break;
case CH6_AHRS_YAW_KP:
ahrs._kp_yaw.set(tuning_value);
break;

View File

@ -303,8 +303,8 @@ public:
k_param_pid_nav_lon, // 234 - remove
k_param_p_alt_hold,
k_param_p_throttle_rate,
k_param_pid_optflow_roll,
k_param_pid_optflow_pitch,
k_param_pid_optflow_roll, // 237 - remove
k_param_pid_optflow_pitch, // 238 - remove
k_param_acro_balance_roll_old, // 239 - remove
k_param_acro_balance_pitch_old, // 240 - remove
k_param_pid_throttle_accel,
@ -457,8 +457,6 @@ public:
AC_P p_throttle_rate;
AC_PID pid_throttle_accel;
AC_PID pid_optflow_roll;
AC_PID pid_optflow_pitch;
AC_P p_loiter_pos;
AC_P p_stabilize_roll;
@ -518,8 +516,6 @@ public:
p_throttle_rate (THROTTLE_RATE_P),
pid_throttle_accel (THROTTLE_ACCEL_P, THROTTLE_ACCEL_I, THROTTLE_ACCEL_D, THROTTLE_ACCEL_IMAX),
pid_optflow_roll (OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX),
pid_optflow_pitch (OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX),
// P controller initial P
//----------------------------------------------------------------------

View File

@ -822,58 +822,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GGROUP(pid_throttle_accel,"THR_ACCEL_", AC_PID),
// @Param: OF_RLL_P
// @DisplayName: Optical Flow based loiter controller roll axis P gain
// @Description: Optical Flow based loiter controller roll axis P gain. Converts the position error from the target point to a roll angle
// @Range: 2.000 3.000
// @User: Standard
// @Param: OF_RLL_I
// @DisplayName: Optical Flow based loiter controller roll axis I gain
// @Description: Optical Flow based loiter controller roll axis I gain. Corrects long-term position error by more persistently rolling left or right
// @Range: 0.250 0.750
// @User: Standard
// @Param: OF_RLL_IMAX
// @DisplayName: Optical Flow based loiter controller roll axis I gain maximum
// @Description: Optical Flow based loiter controller roll axis I gain maximum. Constrains the maximum roll angle that the I term will generate
// @Range: 0 4500
// @Units: Centi-Degrees
// @User: Standard
// @Param: OF_RLL_D
// @DisplayName: Optical Flow based loiter controller roll axis D gain
// @Description: Optical Flow based loiter controller roll axis D gain. Compensates for short-term change in speed in the roll direction
// @Range: 0.100 0.140
// @User: Standard
GGROUP(pid_optflow_roll, "OF_RLL_", AC_PID),
// @Param: OF_PIT_P
// @DisplayName: Optical Flow based loiter controller pitch axis P gain
// @Description: Optical Flow based loiter controller pitch axis P gain. Converts the position error from the target point to a pitch angle
// @Range: 2.000 3.000
// @User: Standard
// @Param: OF_PIT_I
// @DisplayName: Optical Flow based loiter controller pitch axis I gain
// @Description: Optical Flow based loiter controller pitch axis I gain. Corrects long-term position error by more persistently pitching left or right
// @Range: 0.250 0.750
// @User: Standard
// @Param: OF_PIT_IMAX
// @DisplayName: Optical Flow based loiter controller pitch axis I gain maximum
// @Description: Optical Flow based loiter controller pitch axis I gain maximum. Constrains the maximum pitch angle that the I term will generate
// @Range: 0 4500
// @Units: Centi-Degrees
// @User: Standard
// @Param: OF_PIT_D
// @DisplayName: Optical Flow based loiter controller pitch axis D gain
// @Description: Optical Flow based loiter controller pitch axis D gain. Compensates for short-term change in speed in the pitch direction
// @Range: 0.100 0.140
// @User: Standard
GGROUP(pid_optflow_pitch, "OF_PIT_", AC_PID),
// P controllers
//--------------
// @Param: STB_RLL_P

View File

@ -129,9 +129,9 @@
#define CH6_ACRO_YAW_KP 40 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
#define CH6_RELAY 9 // deprecated -- remove
#define CH6_HELI_EXTERNAL_GYRO 13 // TradHeli specific external tail gyro gain
#define CH6_OPTFLOW_KP 17 // optical flow loiter controller's P term (position error to tilt angle)
#define CH6_OPTFLOW_KI 18 // optical flow loiter controller's I term (position error to tilt angle)
#define CH6_OPTFLOW_KD 19 // optical flow loiter controller's D term (position error to tilt angle)
#define CH6_OPTFLOW_KP 17 // deprecated -- remove
#define CH6_OPTFLOW_KI 18 // deprecated -- remove
#define CH6_OPTFLOW_KD 19 // deprecated -- remove
#define CH6_AHRS_YAW_KP 30 // ahrs's compass effect on yaw angle (0 = very low, 1 = very high)
#define CH6_AHRS_KP 31 // accelerometer effect on roll/pitch angle (0=low)
#define CH6_INAV_TC 32 // deprecated -- remove