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

Copter: move LOITER_RATE to 2-axis PI controller 

Also rename LOITER_RATE to VEL_XY for parameters, definitions, variables
mission-4.1.18
Leonard Hall 10 years ago committed by Randy Mackay
parent
1ec5eedd23
commit
e232867661
6 changed files with 34 additions and 82 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. 3
      ArduCopter/ArduCopter.pde
  2. 11
      ArduCopter/Parameters.h
  3. 61
      ArduCopter/Parameters.pde
  4. 21
      ArduCopter/config.h
  5. 5
      ArduCopter/defines.h
  6. 15
      ArduCopter/tuning.pde

3
ArduCopter/ArduCopter.pde
Unescape View File

@ -118,6 +118,7 @@ @@ -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 @@ -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
Unescape View File

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

@ -334,7 +334,7 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -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 = { @@ -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
// @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
// @Units: cm/s/s
// @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
Unescape View File

@ -641,22 +641,19 @@ @@ -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
Unescape View File

@ -122,9 +122,8 @@ @@ -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
Unescape View File

@ -79,19 +79,12 @@ static void tuning() { @@ -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:

Write Preview
Loading…
Cancel
Save