zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: replace APM_PI with AC_P 

Saves about 90bytes of RAM
mission-4.1.18
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
451910fc94
commit
aab9b30bf6
6 changed files with 54 additions and 159 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      ArduCopter/ArduCopter.pde
  2. 34
      ArduCopter/Parameters.h
  3. 86
      ArduCopter/Parameters.pde
  4. 34
      ArduCopter/config.h
  5. 36
      ArduCopter/control_autotune.pde
  6. 2
      ArduCopter/motors.pde

21
ArduCopter/ArduCopter.pde
Unescape View File

@ -108,8 +108,8 @@ @@ -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); @@ -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(){ @@ -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(){ @@ -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(){ @@ -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(){ @@ -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
Unescape View File

@ -264,16 +264,16 @@ public: @@ -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: @@ -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: @@ -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
Unescape View File

@ -820,107 +820,35 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -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
// @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
// @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_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
Unescape View File

@ -511,32 +511,14 @@ @@ -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 @@ @@ -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 @@ @@ -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
Unescape View File

@ -667,19 +667,19 @@ static void autotune_backup_gains_and_initialise() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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
Unescape View File

@ -358,7 +358,7 @@ static void pre_arm_checks(bool display_failure) @@ -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"));
}

Write Preview
Loading…
Cancel
Save