diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1280774b92..e78fdf9c19 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -108,8 +108,8 @@ #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include #include -#include // PI library #include // PID library +#include // P library #include // Attitude control library #include // Attitude control library for traditional helicopter #include // Position control library @@ -677,15 +677,15 @@ static AP_InertialNav inertial_nav(ahrs, barometer, g_gps, gps_glitch); // To-Do: move inertial nav up or other navigation variables down here //////////////////////////////////////////////////////////////////////////////// #if FRAME_CONFIG == HELI_FRAME -AC_AttitudeControl_Heli attitude_control(ahrs, ins, aparm, motors, g.pi_stabilize_roll, g.pi_stabilize_pitch, g.pi_stabilize_yaw, +AC_AttitudeControl_Heli attitude_control(ahrs, ins, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); #else -AC_AttitudeControl attitude_control(ahrs, ins, aparm, motors, g.pi_stabilize_roll, g.pi_stabilize_pitch, g.pi_stabilize_yaw, +AC_AttitudeControl attitude_control(ahrs, ins, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); #endif AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control, - g.pi_alt_hold, g.pid_throttle_rate, g.pid_throttle_accel, - g.pi_loiter_lat, g.pi_loiter_lon, g.pid_loiter_rate_lat, g.pid_loiter_rate_lon); + g.p_alt_hold, g.pid_throttle_rate, g.pid_throttle_accel, + g.p_loiter_pos, g.pid_loiter_rate_lat, g.pid_loiter_rate_lon); static AC_WPNav wp_nav(&inertial_nav, &ahrs, pos_control); static AC_Circle circle_nav(inertial_nav, ahrs, pos_control); @@ -1383,8 +1383,8 @@ static void tuning(){ // Roll, Pitch tuning case CH6_STABILIZE_ROLL_PITCH_KP: - g.pi_stabilize_roll.kP(tuning_value); - g.pi_stabilize_pitch.kP(tuning_value); + g.p_stabilize_roll.kP(tuning_value); + g.p_stabilize_pitch.kP(tuning_value); break; case CH6_RATE_ROLL_PITCH_KP: @@ -1404,7 +1404,7 @@ static void tuning(){ // Yaw tuning case CH6_STABILIZE_YAW_KP: - g.pi_stabilize_yaw.kP(tuning_value); + g.p_stabilize_yaw.kP(tuning_value); break; case CH6_YAW_RATE_KP: @@ -1417,7 +1417,7 @@ static void tuning(){ // Altitude and throttle tuning case CH6_ALTITUDE_HOLD_KP: - g.pi_alt_hold.kP(tuning_value); + g.p_alt_hold.kP(tuning_value); break; case CH6_THROTTLE_RATE_KP: @@ -1442,8 +1442,7 @@ static void tuning(){ // Loiter and navigation tuning case CH6_LOITER_POSITION_KP: - g.pi_loiter_lat.kP(tuning_value); - g.pi_loiter_lon.kP(tuning_value); + g.p_loiter_pos.kP(tuning_value); break; case CH6_LOITER_RATE_KP: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index bc459620c7..ac9771a83c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -264,16 +264,16 @@ public: k_param_pid_rate_roll, k_param_pid_rate_pitch, k_param_pid_rate_yaw, - k_param_pi_stabilize_roll, - k_param_pi_stabilize_pitch, - k_param_pi_stabilize_yaw, - k_param_pi_loiter_lat, - k_param_pi_loiter_lon, + k_param_p_stabilize_roll, + k_param_p_stabilize_pitch, + k_param_p_stabilize_yaw, + k_param_p_loiter_pos, + k_param_p_loiter_lon, // remove k_param_pid_loiter_rate_lat, k_param_pid_loiter_rate_lon, k_param_pid_nav_lat, // 233 - remove k_param_pid_nav_lon, // 234 - remove - k_param_pi_alt_hold, + k_param_p_alt_hold, k_param_pid_throttle_rate, k_param_pid_optflow_roll, k_param_pid_optflow_pitch, @@ -420,12 +420,11 @@ public: AC_PID pid_optflow_roll; AC_PID pid_optflow_pitch; - APM_PI pi_loiter_lat; - APM_PI pi_loiter_lon; - APM_PI pi_stabilize_roll; - APM_PI pi_stabilize_pitch; - APM_PI pi_stabilize_yaw; - APM_PI pi_alt_hold; + AC_P p_loiter_pos; + AC_P p_stabilize_roll; + AC_P p_stabilize_pitch; + AC_P p_stabilize_yaw; + AC_P p_alt_hold; // Note: keep initializers here in the same order as they are declared // above. @@ -484,14 +483,13 @@ public: // PI controller initial P initial I initial // imax //---------------------------------------------------------------------- - pi_loiter_lat (LOITER_P, LOITER_I, LOITER_IMAX), - pi_loiter_lon (LOITER_P, LOITER_I, LOITER_IMAX), + p_loiter_pos (LOITER_POS_P), - pi_stabilize_roll (STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX), - pi_stabilize_pitch (STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX), - pi_stabilize_yaw (STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX), + p_stabilize_roll (STABILIZE_ROLL_P), + p_stabilize_pitch (STABILIZE_PITCH_P), + p_stabilize_yaw (STABILIZE_YAW_P), - pi_alt_hold (ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX) + p_alt_hold (ALT_HOLD_P) { } }; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 6143fc679c..8f0cbe1d9c 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -820,107 +820,35 @@ const AP_Param::Info var_info[] PROGMEM = { // @Range: 0 4500 // @Units: Centi-Degrees/Sec // @User: Standard - GGROUP(pi_stabilize_roll, "STB_RLL_", APM_PI), + GGROUP(p_stabilize_roll, "STB_RLL_", AC_P), // @Param: STB_PIT_P // @DisplayName: Pitch axis stabilize controller P gain // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate // @Range: 3.000 6.000 // @User: Standard - - // @Param: STB_PIT_I - // @DisplayName: Pitch axis stabilize controller I gain - // @Description: Pitch axis stabilize (i.e. angle) controller I gain. Corrects for longer-term difference in desired pitch angle and actual angle - // @Range: 0.000 0.100 - // @User: Standard - - // @Param: STB_PIT_IMAX - // @DisplayName: Pitch axis stabilize controller I gain maximum - // @Description: Pitch axis stabilize (i.e. angle) controller I gain maximum. Constrains the maximum pitch rate that the I term will generate - // @Range: 0 4500 - // @Units: Centi-Degrees/Sec - // @User: Standard - GGROUP(pi_stabilize_pitch, "STB_PIT_", APM_PI), + GGROUP(p_stabilize_pitch, "STB_PIT_", AC_P), // @Param: STB_YAW_P // @DisplayName: Yaw axis stabilize controller P gain // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate // @Range: 3.000 6.000 // @User: Standard - - // @Param: STB_YAW_I - // @DisplayName: Yaw axis stabilize controller I gain - // @Description: Yaw axis stabilize (i.e. angle) controller I gain. Corrects for longer-term difference in desired yaw angle and actual angle - // @Range: 0.000 0.100 - // @User: Standard - - // @Param: STB_YAW_IMAX - // @DisplayName: Yaw axis stabilize controller I gain maximum - // @Description: Yaw axis stabilize (i.e. angle) controller I gain maximum. Constrains the maximum yaw rate that the I term will generate - // @Range: 0 4500 - // @Units: Centi-Degrees/Sec - // @User: Standard - GGROUP(pi_stabilize_yaw, "STB_YAW_", APM_PI), + GGROUP(p_stabilize_yaw, "STB_YAW_", AC_P), // @Param: THR_ALT_P // @DisplayName: Altitude controller P gain // @Description: Altitude controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller // @Range: 1.000 3.000 // @User: Standard - - // @Param: THR_ALT_I - // @DisplayName: Altitude controller I gain - // @Description: Altitude controller I gain. Corrects for longer-term difference in desired altitude and actual altitude - // @Range: 0.000 0.100 - // @User: Standard - - // @Param: THR_ALT_IMAX - // @DisplayName: Altitude controller I gain maximum - // @Description: Altitude controller I gain maximum. Constrains the maximum climb rate rate that the I term will generate - // @Range: 0 500 - // @Units: cm/s - // @User: Standard - GGROUP(pi_alt_hold, "THR_ALT_", APM_PI), + GGROUP(p_alt_hold, "THR_ALT_", AC_P), // @Param: HLD_LAT_P - // @DisplayName: Loiter latitude position controller P gain - // @Description: Loiter latitude position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller + // @DisplayName: Loiter position controller P gain + // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller // @Range: 0.500 2.000 // @User: Standard - - // @Param: HLD_LAT_I - // @DisplayName: Loiter latitude position controller I gain - // @Description: Loiter latitude position controller I gain. Corrects for longer-term distance (in latitude) to the target location - // @Range: 0.000 0.100 - // @User: Standard - - // @Param: HLD_LAT_IMAX - // @DisplayName: Loiter latitude position controller I gain maximum - // @Description: Loiter latitude position controller I gain maximum. Constrains the maximum desired speed that the I term will generate - // @Range: 0 3000 - // @Units: cm/s - // @User: Standard - GGROUP(pi_loiter_lat, "HLD_LAT_", APM_PI), - - // @Param: HLD_LON_P - // @DisplayName: Loiter longitude position controller P gain - // @Description: Loiter longitude position controller P gain. Converts the distance (in the longitude direction) to the target location into a desired speed which is then passed to the loiter longitude rate controller - // @Range: 0.500 2.000 - // @User: Standard - - // @Param: HLD_LON_I - // @DisplayName: Loiter longitude position controller I gain - // @Description: Loiter longitude position controller I gain. Corrects for longer-term distance (in longitude direction) to the target location - // @Range: 0.000 0.100 - // @User: Standard - - // @Param: HLD_LON_IMAX - // @DisplayName: Loiter longitudeposition controller I gain maximum - // @Description: Loiter longitudeposition controller I gain maximum. Constrains the maximum desired speed that the I term will generate - // @Range: 0 3000 - // @Units: cm/s - // @User: Standard - GGROUP(pi_loiter_lon, "HLD_LON_", APM_PI), + GGROUP(p_loiter_pos, "HLD_LAT_", AC_P), // variables not in the g class which contain EEPROM saved variables diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 27167ce71b..1f940985bd 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -511,32 +511,14 @@ #ifndef STABILIZE_ROLL_P # define STABILIZE_ROLL_P 4.5f #endif -#ifndef STABILIZE_ROLL_I - # define STABILIZE_ROLL_I 0.0f -#endif -#ifndef STABILIZE_ROLL_IMAX - # define STABILIZE_ROLL_IMAX 0 -#endif #ifndef STABILIZE_PITCH_P # define STABILIZE_PITCH_P 4.5f #endif -#ifndef STABILIZE_PITCH_I - # define STABILIZE_PITCH_I 0.0f -#endif -#ifndef STABILIZE_PITCH_IMAX - # define STABILIZE_PITCH_IMAX 0 -#endif #ifndef STABILIZE_YAW_P # define STABILIZE_YAW_P 4.5f #endif -#ifndef STABILIZE_YAW_I - # define STABILIZE_YAW_I 0.0f -#endif -#ifndef STABILIZE_YAW_IMAX - # define STABILIZE_YAW_IMAX 0 -#endif // RTL Mode #ifndef RTL_ALT_FINAL @@ -631,14 +613,8 @@ ////////////////////////////////////////////////////////////////////////////// // Loiter position control gains // -#ifndef LOITER_P - # define LOITER_P 1.0f -#endif -#ifndef LOITER_I - # define LOITER_I 0.0f -#endif -#ifndef LOITER_IMAX - # define LOITER_IMAX 0 +#ifndef LOITER_POS_P + # define LOITER_POS_P 1.0f #endif ////////////////////////////////////////////////////////////////////////////// @@ -682,12 +658,6 @@ #ifndef ALT_HOLD_P # define ALT_HOLD_P 1.0f #endif -#ifndef ALT_HOLD_I - # define ALT_HOLD_I 0.0f -#endif -#ifndef ALT_HOLD_IMAX - # define ALT_HOLD_IMAX 300 -#endif // RATE control #ifndef THROTTLE_RATE_P diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 237dc47adb..c96eca7ce8 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -667,19 +667,19 @@ static void autotune_backup_gains_and_initialise() orig_roll_rp = g.pid_rate_roll.kP(); orig_roll_ri = g.pid_rate_roll.kI(); orig_roll_rd = g.pid_rate_roll.kD(); - orig_roll_sp = g.pi_stabilize_roll.kP(); + orig_roll_sp = g.p_stabilize_roll.kP(); orig_pitch_rp = g.pid_rate_pitch.kP(); orig_pitch_ri = g.pid_rate_pitch.kI(); orig_pitch_rd = g.pid_rate_pitch.kD(); - orig_pitch_sp = g.pi_stabilize_pitch.kP(); + orig_pitch_sp = g.p_stabilize_pitch.kP(); // initialise tuned pid values tune_roll_rp = g.pid_rate_roll.kP(); tune_roll_rd = g.pid_rate_roll.kD(); - tune_roll_sp = g.pi_stabilize_roll.kP(); + tune_roll_sp = g.p_stabilize_roll.kP(); tune_pitch_rp = g.pid_rate_pitch.kP(); tune_pitch_rd = g.pid_rate_pitch.kD(); - tune_pitch_sp = g.pi_stabilize_pitch.kP(); + tune_pitch_sp = g.p_stabilize_pitch.kP(); Log_Write_Event(DATA_AUTOTUNE_INITIALISED); } @@ -693,11 +693,11 @@ static void autotune_load_orig_gains() g.pid_rate_roll.kP(orig_roll_rp); g.pid_rate_roll.kI(orig_roll_ri); g.pid_rate_roll.kD(orig_roll_rd); - g.pi_stabilize_roll.kP(orig_roll_sp); + g.p_stabilize_roll.kP(orig_roll_sp); g.pid_rate_pitch.kP(orig_pitch_rp); g.pid_rate_pitch.kI(orig_pitch_ri); g.pid_rate_pitch.kD(orig_pitch_rd); - g.pi_stabilize_pitch.kP(orig_pitch_sp); + g.p_stabilize_pitch.kP(orig_pitch_sp); } } @@ -709,11 +709,11 @@ static void autotune_load_tuned_gains() g.pid_rate_roll.kP(tune_roll_rp); g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_RP_RATIO_FINAL); g.pid_rate_roll.kD(tune_roll_rd); - g.pi_stabilize_roll.kP(tune_roll_sp); + g.p_stabilize_roll.kP(tune_roll_sp); g.pid_rate_pitch.kP(tune_pitch_rp); g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_RP_RATIO_FINAL); g.pid_rate_pitch.kD(tune_pitch_rd); - g.pi_stabilize_pitch.kP(tune_pitch_sp); + g.p_stabilize_pitch.kP(tune_pitch_sp); }else{ // log an error message and fail the autotune Log_Write_Error(ERROR_SUBSYSTEM_AUTOTUNE,ERROR_CODE_AUTOTUNE_BAD_GAINS); @@ -730,11 +730,11 @@ static void autotune_load_intra_test_gains() g.pid_rate_roll.kP(orig_roll_rp); g.pid_rate_roll.kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); g.pid_rate_roll.kD(orig_roll_rd); - g.pi_stabilize_roll.kP(orig_roll_sp); + g.p_stabilize_roll.kP(orig_roll_sp); g.pid_rate_pitch.kP(orig_pitch_rp); g.pid_rate_pitch.kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); g.pid_rate_pitch.kD(orig_pitch_rd); - g.pi_stabilize_pitch.kP(orig_pitch_sp); + g.p_stabilize_pitch.kP(orig_pitch_sp); }else{ // log an error message and fail the autotune Log_Write_Error(ERROR_SUBSYSTEM_AUTOTUNE,ERROR_CODE_AUTOTUNE_BAD_GAINS); @@ -750,7 +750,7 @@ static void autotune_load_twitch_gains() g.pid_rate_roll.kP(tune_roll_rp); g.pid_rate_roll.kI(tune_roll_rp*0.01f); g.pid_rate_roll.kD(tune_roll_rd); - g.pi_stabilize_roll.kP(tune_roll_sp); + g.p_stabilize_roll.kP(tune_roll_sp); }else{ // log an error message and fail the autotune Log_Write_Error(ERROR_SUBSYSTEM_AUTOTUNE,ERROR_CODE_AUTOTUNE_BAD_GAINS); @@ -760,7 +760,7 @@ static void autotune_load_twitch_gains() g.pid_rate_pitch.kP(tune_pitch_rp); g.pid_rate_pitch.kI(tune_pitch_rp*0.01f); g.pid_rate_pitch.kD(tune_pitch_rd); - g.pi_stabilize_pitch.kP(tune_pitch_sp); + g.p_stabilize_pitch.kP(tune_pitch_sp); }else{ // log an error message and fail the autotune Log_Write_Error(ERROR_SUBSYSTEM_AUTOTUNE,ERROR_CODE_AUTOTUNE_BAD_GAINS); @@ -789,22 +789,22 @@ static void autotune_save_tuning_gains() g.pid_rate_pitch.save_gains(); // stabilize roll - g.pi_stabilize_roll.kP(tune_roll_sp); - g.pi_stabilize_roll.save_gains(); + g.p_stabilize_roll.kP(tune_roll_sp); + g.p_stabilize_roll.save_gains(); // stabilize pitch - g.pi_stabilize_pitch.save_gains(); - g.pi_stabilize_pitch.kP(tune_pitch_sp); + g.p_stabilize_pitch.save_gains(); + g.p_stabilize_pitch.kP(tune_pitch_sp); // resave pids to originals in case the autotune is run again orig_roll_rp = g.pid_rate_roll.kP(); orig_roll_ri = g.pid_rate_roll.kI(); orig_roll_rd = g.pid_rate_roll.kD(); - orig_roll_sp = g.pi_stabilize_roll.kP(); + orig_roll_sp = g.p_stabilize_roll.kP(); orig_pitch_rp = g.pid_rate_pitch.kP(); orig_pitch_ri = g.pid_rate_pitch.kI(); orig_pitch_rd = g.pid_rate_pitch.kD(); - orig_pitch_sp = g.pi_stabilize_pitch.kP(); + orig_pitch_sp = g.p_stabilize_pitch.kP(); // log save gains event Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS); diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 1d8074a8ce..b6a02ea1ae 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -358,7 +358,7 @@ static void pre_arm_checks(bool display_failure) } // acro balance parameter check - if ((g.acro_balance_roll > g.pi_stabilize_roll.kP()) || (g.acro_balance_pitch > g.pi_stabilize_pitch.kP())) { + if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: ACRO_BAL_ROLL/PITCH")); }