aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: move LOITER_RATE to 2-axis PI controller

Browse Source 
Also rename LOITER_RATE to VEL_XY for parameters, definitions, variables
This commit is contained in:
Leonard Hall 2015-01-29 15:51:21 +09:00 committed by Randy Mackay
parent 1ec5eedd23
commit e232867661
6 changed files with 34 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -118,6 +118,7 @@
#include <AP_Mission.h> // Mission command library
#include <AP_Rally.h> // Rally point library
#include <AC_PID.h> // PID library
#include <AC_PI_2D.h> // PID library (2-axis)
#include <AC_HELI_PID.h> // Heli specific Rate PID library
#include <AC_P.h> // P library
#include <AC_AttitudeControl.h> // Attitude control library
@ -623,7 +624,7 @@ AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p
#endif
AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_throttle_rate, g.pid_throttle_accel,
g.p_loiter_pos, g.pid_loiter_rate_lat, g.pid_loiter_rate_lon);
g.p_loiter_pos, g.pi_vel_xy);
static AC_WPNav wp_nav(inertial_nav, ahrs, pos_control, attitude_control);
static AC_Circle circle_nav(inertial_nav, ahrs, pos_control);

11
ArduCopter/Parameters.h
View File

@ -301,8 +301,8 @@ public:
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_loiter_rate_lat, // remove
k_param_pid_loiter_rate_lon, // remove
k_param_pid_nav_lat, // 233 - remove
k_param_pid_nav_lon, // 234 - remove
k_param_p_alt_hold,
@ -317,6 +317,7 @@ public:
k_param_acro_yaw_p, // 244
k_param_autotune_axis_bitmask, // 245
k_param_autotune_aggressiveness, // 246
k_param_pi_vel_xy, // 247
// 254,255: reserved
};
@ -452,8 +453,7 @@ public:
AC_PID pid_rate_pitch;
AC_PID pid_rate_yaw;
#endif
AC_PID pid_loiter_rate_lat;
AC_PID pid_loiter_rate_lon;
AC_PI_2D pi_vel_xy;
AC_P p_throttle_rate;
AC_PID pid_throttle_accel;
@ -515,8 +515,7 @@ public:
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS),
pid_loiter_rate_lat (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX, LOITER_RATE_FILT_HZ,WPNAV_LOITER_UPDATE_TIME),
pid_loiter_rate_lon (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX, LOITER_RATE_FILT_HZ,WPNAV_LOITER_UPDATE_TIME),
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),
p_throttle_rate (THROTTLE_RATE_P),
pid_throttle_accel (THROTTLE_ACCEL_P,THROTTLE_ACCEL_I, THROTTLE_ACCEL_D,THROTTLE_ACCEL_IMAX, THROTTLE_ACCEL_FILT_HZ, MAIN_LOOP_SECONDS),

61
ArduCopter/Parameters.pde
View File

@ -334,7 +334,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @DisplayName: Channel 6 Tuning
// @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob
// @User: Standard
// @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,14:Altitude Hold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,42:Loiter Speed,12:Loiter Pos kP,22:Loiter Rate kP,28:Loiter Rate kI,23:Loiter Rate kD,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,30:AHRS Yaw kP,31:AHRS kP,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF
// @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,14:Altitude Hold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,42:Loiter Speed,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,30:AHRS Yaw kP,31:AHRS kP,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF
GSCALAR(radio_tuning, "TUNE", 0),
// @Param: TUNE_LOW
@ -680,65 +680,28 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(pid_rate_yaw, "RATE_YAW_", AC_PID),
#endif
// @Param: LOITER_LAT_P
// @DisplayName: Loiter latitude rate controller P gain
// @Description: Loiter latitude rate controller P gain. Converts the difference between desired speed and actual speed into a lean angle in the latitude direction
// @Param: VEL_XY_P
// @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: LOITER_LAT_I
// @DisplayName: Loiter latitude rate controller I gain
// @Description: Loiter latitude rate controller I gain. Corrects long-term difference in desired speed and actual speed in the latitude direction
// @Param: VEL_XY_I
// @DisplayName: Velocity (horizontal) I gain
// @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: LOITER_LAT_IMAX
// @DisplayName: Loiter rate controller I gain maximum
// @Description: Loiter rate controller I gain maximum. Constrains the lean angle that the I gain will output
// @Param: VEL_XY_IMAX
// @DisplayName: Velocity (horizontal) integrator maximum
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: Centi-Degrees
// @Units: cm/s/s
// @User: Advanced
// @Param: LOITER_LAT_D
// @DisplayName: Loiter latitude rate controller D gain
// @Description: Loiter latitude rate controller D gain. Compensates for short-term change in desired speed vs actual speed
// @Range: 0.0 0.6
// @Increment: 0.01
// @User: Advanced
GGROUP(pid_loiter_rate_lat, "LOITER_LAT_", AC_PID),
// @Param: LOITER_LON_P
// @DisplayName: Loiter longitude rate controller P gain
// @Description: Loiter longitude rate controller P gain. Converts the difference between desired speed and actual speed into a lean angle in the longitude direction
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: LOITER_LON_I
// @DisplayName: Loiter longitude rate controller I gain
// @Description: Loiter longitude rate controller I gain. Corrects long-term difference in desired speed and actual speed in the longitude direction
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: LOITER_LON_IMAX
// @DisplayName: Loiter longitude rate controller I gain maximum
// @Description: Loiter longitude rate controller I gain maximum. Constrains the lean angle that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: Centi-Degrees
// @User: Advanced
// @Param: LOITER_LON_D
// @DisplayName: Loiter longituderate controller D gain
// @Description: Loiter longitude rate controller D gain. Compensates for short-term change in desired speed vs actual speed
// @Range: 0.0 0.6
// @Increment: 0.01
// @User: Advanced
GGROUP(pid_loiter_rate_lon, "LOITER_LON_", AC_PID),
GGROUP(pi_vel_xy, "VEL_XY_", AC_PI_2D),
// @Param: THR_RATE_P
// @DisplayName: Throttle rate controller P gain

21
ArduCopter/config.h
View File

@ -641,22 +641,19 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter rate control gains
// Velocity (horizontal) gains
//
#ifndef LOITER_RATE_P
# define LOITER_RATE_P 1.0f
#ifndef VEL_XY_P
# define VEL_XY_P 1.0f
#endif
#ifndef LOITER_RATE_I
# define LOITER_RATE_I 0.5f
#ifndef VEL_XY_I
# define VEL_XY_I 0.5f
#endif
#ifndef LOITER_RATE_D
# define LOITER_RATE_D 0.0f
#ifndef VEL_XY_IMAX
# define VEL_XY_IMAX 1000
#endif
#ifndef LOITER_RATE_IMAX
# define LOITER_RATE_IMAX 1000 // maximum acceleration from I term build-up in cm/s/s
#endif
#ifndef LOITER_RATE_FILT_HZ
# define LOITER_RATE_FILT_HZ 5.0f
#ifndef VEL_XY_FILT_HZ
# define VEL_XY_FILT_HZ 5.0f
#endif
//////////////////////////////////////////////////////////////////////////////

5
ArduCopter/defines.h
View File

@ -122,9 +122,8 @@
#define CH6_THROTTLE_ACCEL_KI 35 // accel based throttle controller's I term
#define CH6_THROTTLE_ACCEL_KD 36 // accel based throttle controller's D term
#define CH6_LOITER_POSITION_KP 12 // loiter distance controller's P term (position error to speed)
#define CH6_LOITER_RATE_KP 22 // loiter rate controller's P term (speed error to tilt angle)
#define CH6_LOITER_RATE_KI 28 // loiter rate controller's I term (speed error to tilt angle)
#define CH6_LOITER_RATE_KD 23 // loiter rate controller's D term (speed error to tilt angle)
#define CH6_VEL_XY_KP 22 // loiter rate controller's P term (speed error to tilt angle)
#define CH6_VEL_XY_KI 28 // loiter rate controller's I term (speed error to tilt angle)
#define CH6_WP_SPEED 10 // maximum speed to next way point (0 to 10m/s)
#define CH6_ACRO_RP_KP 25 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
#define CH6_ACRO_YAW_KP 40 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate

15
ArduCopter/tuning.pde
View File

@ -79,19 +79,12 @@ static void tuning() {
g.p_loiter_pos.kP(tuning_value);
break;
case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon.kP(tuning_value);
g.pid_loiter_rate_lat.kP(tuning_value);
case CH6_VEL_XY_KP:
g.pi_vel_xy.kP(tuning_value);
break;
case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon.kI(tuning_value);
g.pid_loiter_rate_lat.kI(tuning_value);
break;
case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon.kD(tuning_value);
g.pid_loiter_rate_lat.kD(tuning_value);
case CH6_VEL_XY_KI:
g.pi_vel_xy.kI(tuning_value);
break;
case CH6_WP_SPEED: