Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: replace APM_PI with AC_P 

Saves about 90bytes of RAM
This commit is contained in:
Randy Mackay 2014-02-14 16:08:59 +09:00 committed by Andrew Tridgell
parent 451910fc94
commit aab9b30bf6
6 changed files with 54 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduCopter/ArduCopter.pde
View File

@ -108,8 +108,8 @@
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS.h>
#include <AP_NavEKF.h>
#include <APM_PI.h> // PI library
#include <AC_PID.h> // PID library
#include <AC_P.h> // P library
#include <AC_AttitudeControl.h> // Attitude control library
#include <AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_PosControl.h> // 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:

34
ArduCopter/Parameters.h
View File

@ -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)
{
}
};

86
ArduCopter/Parameters.pde
View File

@ -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

34
ArduCopter/config.h
View File

@ -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

36
ArduCopter/control_autotune.pde
View File

@ -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);

2
ArduCopter/motors.pde
View File

@ -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"));
}