diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 06682964c4..db296b1d7d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ea91a0d2b2..65f17f8713 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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 //---------------------------------------------------------------------- diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index a77f6e4ad1..27f008d396 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index fcff903ba3..cdcf8fbcde 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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