mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
Copter: precision landing velocity PI object
Follow up changes required to actually use this PI during landing
This commit is contained in:
parent
f4b152f0e3
commit
7db77482f5
@ -851,6 +851,28 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
|
||||
// @User: Standard
|
||||
GGROUP(p_pos_xy, "POS_XY_", AC_P),
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
// @Param: PRECLNDVEL_P
|
||||
// @DisplayName: Precision landing velocity controller P gain
|
||||
// @Description: Precision landing velocity controller P gain
|
||||
// @Range: 0.100 5.000
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: PRECLNDVEL_I
|
||||
// @DisplayName: Precision landing velocity controller I gain
|
||||
// @Description: Precision landing velocity controller I gain
|
||||
// @Range: 0.100 5.000
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: PRECLNDVEL_IMAX
|
||||
// @DisplayName: Precision landing velocity controller I gain maximum
|
||||
// @Description: Precision landing velocity controller I gain maximum
|
||||
// @Range: 0 1000
|
||||
// @Units: cm/s
|
||||
// @User: Standard
|
||||
GGROUP(pi_precland, "PLAND_", AC_PI_2D),
|
||||
#endif
|
||||
|
||||
// variables not in the g class which contain EEPROM saved variables
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
|
@ -348,6 +348,7 @@ public:
|
||||
k_param_rtl_climb_min,
|
||||
k_param_rpm_sensor,
|
||||
k_param_autotune_min_d, // 251
|
||||
k_param_pi_precland, // 252
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
@ -500,6 +501,10 @@ public:
|
||||
AC_P p_vel_z;
|
||||
AC_PID pid_accel_z;
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
AC_PI_2D pi_precland;
|
||||
#endif
|
||||
|
||||
AC_P p_pos_xy;
|
||||
AC_P p_stabilize_roll;
|
||||
AC_P p_stabilize_pitch;
|
||||
@ -570,6 +575,10 @@ public:
|
||||
p_vel_z (VEL_Z_P),
|
||||
pid_accel_z (ACCEL_Z_P, ACCEL_Z_I, ACCEL_Z_D, ACCEL_Z_IMAX, ACCEL_Z_FILT_HZ, MAIN_LOOP_SECONDS),
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
pi_precland (PRECLAND_P, PRECLAND_I, PRECLAND_IMAX, VEL_XY_FILT_HZ, PRECLAND_UPDATE_TIME),
|
||||
#endif
|
||||
|
||||
// P controller initial P
|
||||
//----------------------------------------------------------------------
|
||||
p_pos_xy (POS_XY_P),
|
||||
|
Loading…
Reference in New Issue
Block a user