From d92e66863ac057a4a691d482b8566081700e9e1e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Apr 2020 15:44:40 +0200 Subject: [PATCH] mc_pos_control: migrate MPC_*_VEL_* parameter to acceleration scaling MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Before #14212 the velocity control gains used in the multicopter position controller were defined as a scale between velocity error in one axis (or it's integral and derivative respectively) and the unit thrust vector. The problem with this is that the normalization of the unit thrust vector changes per vehicle or even vehicle configuration as 0 and 100% thrust get a different physical response. That's why the gains are now defined as scale between velocity error (integral/derivative) and the output acceleration in m/s². --- .../init.d-posix/1040_standard_vtol | 6 +-- .../init.d-posix/1041_tailsitter | 8 ++-- .../px4fmu_common/init.d-posix/1042_tiltrotor | 8 ++-- .../init.d-posix/6011_typhoon_h480 | 4 +- ROMFS/px4fmu_common/init.d-posix/rcS | 10 ++--- .../init.d/airframes/1002_standard_vtol.hil | 10 ++--- .../airframes/13006_vtol_standard_delta | 2 +- .../init.d/airframes/13009_vtol_spt_ranger | 4 +- .../init.d/airframes/13013_deltaquad | 2 +- .../init.d/airframes/4016_holybro_px4vision | 6 +-- .../init.d/airframes/4052_holybro_qav250 | 2 +- .../init.d/airframes/4070_aerofc | 6 +-- ROMFS/px4fmu_common/init.d/airframes/4071_ifo | 11 +++-- .../px4fmu_common/init.d/airframes/4073_ifo-s | 11 +++-- .../px4fmu_common/init.d/airframes/4250_teal | 12 +++--- .../init.d/airframes/4500_clover4 | 6 +-- .../init.d/airframes/4900_crazyflie | 4 +- .../init.d/airframes/6002_draco_r | 11 +++-- src/lib/parameters/param_translation.cpp | 18 ++++++++ src/lib/parameters/param_translation.h | 1 + .../mc_pos_control/Takeoff/Takeoff.cpp | 5 ++- .../mc_pos_control/Takeoff/Takeoff.hpp | 2 +- .../mc_pos_control/Takeoff/TakeoffTest.cpp | 3 +- .../mc_pos_control/mc_pos_control_main.cpp | 24 +++++------ .../mc_pos_control/mc_pos_control_params.c | 43 ++++++++++++------- 25 files changed, 123 insertions(+), 96 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index d05d489aa6..0de4a44fac 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -25,9 +25,9 @@ then param set MPC_THR_MIN 0.1 param set MPC_TKO_SPEED 1 param set MPC_XY_P 0.8 - param set MPC_XY_VEL_D 0.005 - param set MPC_XY_VEL_I 0.2 - param set MPC_XY_VEL_P 0.15 + param set MPC_XY_VEL_P_ACC 3 + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_D_ACC 0.1 param set MPC_Z_VEL_MAX_DN 1.5 param set NAV_ACC_RAD 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter index 04d76a4546..5f854e45e4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter @@ -25,11 +25,11 @@ then param set MPC_THR_MIN 0.1 param set MPC_TKO_SPEED 1 param set MPC_XY_P 0.15 - param set MPC_XY_VEL_D 0.005 - param set MPC_XY_VEL_I 0.2 - param set MPC_XY_VEL_P 0.05 + param set MPC_XY_VEL_D_ACC 0.1 + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_P_ACC 1 param set MPC_Z_VEL_MAX_DN 1.5 - param set MPC_Z_VEL_P 0.8 + param set MPC_Z_VEL_P_ACC 16 param set NAV_ACC_RAD 5 param set NAV_LOITER_RAD 80 diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index ce6c66bf69..fe8664b4f4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -25,11 +25,11 @@ then param set MPC_THR_MIN 0.1 param set MPC_TKO_SPEED 1 param set MPC_XY_P 0.15 - param set MPC_XY_VEL_D 0.005 - param set MPC_XY_VEL_I 0.2 - param set MPC_XY_VEL_P 0.05 + param set MPC_XY_VEL_D_ACC 0.1 + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_P_ACC 1 param set MPC_Z_VEL_MAX_DN 1.5 - param set MPC_Z_VEL_P 0.8 + param set MPC_Z_VEL_P_ACC 16 param set NAV_ACC_RAD 5 param set NAV_LOITER_RAD 80 diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index 8409b5f813..4955f3b578 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -15,8 +15,8 @@ then param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.1 param set MC_ROLL_P 6.0 - param set MPC_XY_VEL_I 0.2 - param set MPC_XY_VEL_P 0.15 + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_P_ACC 3 param set RTL_DESCEND_ALT 10 param set RTL_LAND_DELAY 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 142e404331..d6980ea2e1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -161,12 +161,12 @@ then param set MPC_ALT_MODE 0 param set MPC_HOLD_MAX_Z 2 - param set MPC_Z_VEL_I 0.15 - param set MPC_Z_VEL_P 0.6 + param set MPC_Z_VEL_P_ACC 12.0 + param set MPC_Z_VEL_I_ACC 3.0 param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P 0.2 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.016 + param set MPC_XY_VEL_P_ACC 4.0 + param set MPC_XY_VEL_I_ACC 0.4 + param set MPC_XY_VEL_D_ACC 0.32 param set MPC_SPOOLUP_TIME 0.5 param set MPC_TKO_RAMP_T 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index 0f5a6c24f4..97076c12d0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -39,12 +39,12 @@ then param set MPC_THR_MIN 0.1 param set MPC_TKO_SPEED 1 param set MPC_XY_P 0.8 - param set MPC_XY_VEL_D 0.005 - param set MPC_XY_VEL_I 0.2 - param set MPC_XY_VEL_P 0.15 - param set MPC_Z_VEL_I 0.15 + param set MPC_XY_VEL_D_ACC 0.1 + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_P_ACC 3 + param set MPC_Z_VEL_P_ACC 12 + param set MPC_Z_VEL_I_ACC 3 param set MPC_Z_VEL_MAX_DN 1.5 - param set MPC_Z_VEL_P 0.6 param set NAV_ACC_RAD 5 param set NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta b/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta index 0ec75ec36d..f627854683 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta +++ b/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta @@ -41,7 +41,7 @@ then param set MC_YAWRATE_MAX 50 param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_P_ACC 2 param set MPC_ACC_HOR_MAX 2 param set MPC_YAWRAUTO_MAX 20 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger b/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger index ed124fd3e1..5edd067634 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger +++ b/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger @@ -55,9 +55,9 @@ then param set MPC_TKO_SPEED 1 param set MPC_XY_P 0.3 param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.05 + param set MPC_XY_VEL_P_ACC 1 param set MPC_Z_P 0.5 - param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_P_ACC 2 param set MPC_YAWRAUTO_MAX 40 param set NAV_ACC_RAD 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 579ca6bbb3..5078ba2142 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -93,7 +93,7 @@ then param set MIS_TAKEOFF_ALT 15 param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_P_ACC 2 param set MPC_XY_VEL_MAX 5 param set MPC_ACC_HOR_MAX 2 param set MPC_LAND_SPEED 1.2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index f6a262c1f7..c044067d46 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -92,11 +92,11 @@ then param set MPC_VEL_MANUAL 5 param set MPC_XY_CRUISE 5 param set MPC_XY_VEL_MAX 5 - param set MPC_XY_VEL_P 0.079 + param set MPC_XY_VEL_P_ACC 1.58 param set MPC_XY_TRAJ_P 0.3 param set MPC_Z_TRAJ_P 0.3 - param set MPC_Z_VEL_I 0.15 - param set MPC_Z_VEL_P 0.25 + param set MPC_Z_VEL_P_ACC 5 + param set MPC_Z_VEL_I_ACC 3 param set MPC_LAND_ALT1 3 param set MPC_LAND_ALT2 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 index bc188ee1e6..e1ae942109 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 @@ -48,7 +48,7 @@ then param set MPC_THR_CURVE 1 param set MPC_THR_HOVER 0.25 param set MPC_THR_MIN 0.05 - param set MPC_Z_VEL_I 0.085 + param set MPC_Z_VEL_I_ACC 1.7 param set PWM_MAX 1950 param set PWM_MIN 1050 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc b/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc index 98a37ae4fa..e6caf85a54 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc +++ b/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc @@ -56,12 +56,12 @@ then param set MPC_THR_MIN 0.1000 param set MPC_XY_CRUISE 8.0000 param set MPC_XY_P 1.5000 - param set MPC_XY_VEL_P 0.1500 + param set MPC_XY_VEL_P_ACC 3 param set MPC_Z_P 1.5000 - param set MPC_Z_VEL_I 0.1500 + param set MPC_Z_VEL_P_ACC 16 + param set MPC_Z_VEL_I_ACC 3 param set MPC_Z_VEL_MAX_DN 4.0000 param set MPC_Z_VEL_MAX_UP 5.0000 - param set MPC_Z_VEL_P 0.8000 # TELEM2 config param set MAV_1_CONFIG 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index 3e7ede9a90..4848a476c1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -81,18 +81,17 @@ then # Position control param set MPC_Z_P 1.00000 - param set MPC_Z_VEL_P 0.20000 - param set MPC_Z_VEL_I 0.02000 - param set MPC_Z_VEL_D 0.00000 + param set MPC_Z_VEL_P_ACC 4 + param set MPC_Z_VEL_I_ACC 0.4 param set MPC_THR_MIN 0.06000 param set MPC_THR_HOVER 0.3000 param set MIS_TAKEOFF_ALT 1.1000 param set MPC_XY_P 1.7000 - param set MPC_XY_VEL_P 0.1300 - param set MPC_XY_VEL_I 0.0600 - param set MPC_XY_VEL_D 0.0100 + param set MPC_XY_VEL_P_ACC 2.6 + param set MPC_XY_VEL_I_ACC 1.2 + param set MPC_XY_VEL_D_ACC 0.2 param set MPC_TKO_RAMP_T 1.0000 param set MPC_TKO_SPEED 1.1000 param set MPC_VEL_MANUAL 3.0000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s index b1a7e7952e..b78714244e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s +++ b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s @@ -90,18 +90,17 @@ then # Position control param set MPC_Z_P 1.10000 - param set MPC_Z_VEL_P 0.30000 - param set MPC_Z_VEL_I 0.05000 - param set MPC_Z_VEL_D 0.00000 + param set MPC_Z_VEL_P_ACC 6 + param set MPC_Z_VEL_I_ACC 1 param set MPC_THR_MIN 0.06000 param set MPC_THR_HOVER 0.4400 param set MIS_TAKEOFF_ALT 1.1000 param set MPC_XY_P 1.7000 - param set MPC_XY_VEL_P 0.1300 - param set MPC_XY_VEL_I 0.0600 - param set MPC_XY_VEL_D 0.0100 + param set MPC_XY_VEL_P_ACC 2.6 + param set MPC_XY_VEL_I_ACC 1.2 + param set MPC_XY_VEL_D_ACC 0.2 param set MPC_TKO_RAMP_T 1.0000 param set MPC_TKO_SPEED 1.1000 param set MPC_VEL_MANUAL 3.0000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal index 0abcf9df68..a91b588630 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ b/ROMFS/px4fmu_common/init.d/airframes/4250_teal @@ -153,15 +153,15 @@ then param set MPC_ACC_HOR_MAX 15 param set MPC_XY_P 1.15 - param set MPC_XY_VEL_P 0.14 - param set MPC_XY_VEL_I 0.014 - param set MPC_XY_VEL_D 0.014 + param set MPC_XY_VEL_P_ACC 2.8 + param set MPC_XY_VEL_I_ACC 0.28 + param set MPC_XY_VEL_D_ACC 0.28 param set MPC_XY_VEL_MAX 26.5 param set MPC_Z_P 0.85 - param set MPC_Z_VEL_P 0.25 - param set MPC_Z_VEL_I 0.085 - param set MPC_Z_VEL_D 0.02 + param set MPC_Z_VEL_P_ACC 5 + param set MPC_Z_VEL_I_ACC 1.7 + param set MPC_Z_VEL_D_ACC 0.4 # Documentation says limit is 8.0, but does not seem to be enforced in code. param set MPC_Z_VEL_MAX_UP 20 param set MPC_Z_VEL_MAX_DN 2.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 index 4e46393565..7a289ba184 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 +++ b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 @@ -27,9 +27,9 @@ then param set MC_ROLLRATE_I 0.037 param set MC_ROLLRATE_D 0.0044 param set MC_ROLL_P 8.5 - param set MPC_XY_VEL_P 0.11 - param set MPC_XY_VEL_D 0.013 + param set MPC_XY_VEL_P_ACC 2.2 + param set MPC_XY_VEL_D_ACC 0.26 param set MPC_XY_P 1.1 - param set MPC_Z_VEL_P 0.24 + param set MPC_Z_VEL_P_ACC 4.8 param set MPC_Z_P 1.2 fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index 4719f1cdb3..228ceb170e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -53,8 +53,8 @@ then param set MPC_THR_HOVER 0.7 param set MPC_THR_MAX 1 param set MPC_Z_P 1.5 - param set MPC_Z_VEL_I 0.3 - param set MPC_Z_VEL_P 0.4 + param set MPC_Z_VEL_P_ACC 8 + param set MPC_Z_VEL_I_ACC 6 param set MPC_HOLD_MAX_XY 0.1 param set MPC_MAX_FLOW_HGT 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r index 2b0b3f4ade..9c066d8c7e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r @@ -63,14 +63,13 @@ then param set MPC_THR_HOVER 0.3000 # altitude control gains param set MPC_Z_P 1.00000 - param set MPC_Z_VEL_P 0.20000 - param set MPC_Z_VEL_I 0.02000 - param set MPC_Z_VEL_D 0.00000 + param set MPC_Z_VEL_P_ACC 4 + param set MPC_Z_VEL_I_ACC 0.4 # position control gains param set MPC_XY_P 0.9500 - param set MPC_XY_VEL_P 0.0900 - param set MPC_XY_VEL_I 0.0200 - param set MPC_XY_VEL_D 0.0100 + param set MPC_XY_VEL_P_ACC 1.8 + param set MPC_XY_VEL_I_ACC 0.4 + param set MPC_XY_VEL_D_ACC 0.2 # etc gains param set MPC_TKO_RAMP_T 0.4000 param set MPC_TKO_SPEED 1.5000 diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index b9d793af99..690dea7b71 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -41,6 +41,15 @@ bool param_modify_on_import(bson_node_t node) { + // migrate MPC_*_VEL_* -> MPC_*_VEL_*_ACC (2020-04-27). This can be removed after the next release (current release=1.10) + if (node->type == BSON_DOUBLE) { + param_migrate_velocity_gain(node, "MPC_XY_VEL_P"); + param_migrate_velocity_gain(node, "MPC_XY_VEL_I"); + param_migrate_velocity_gain(node, "MPC_XY_VEL_D"); + param_migrate_velocity_gain(node, "MPC_Z_VEL_P"); + param_migrate_velocity_gain(node, "MPC_Z_VEL_I"); + param_migrate_velocity_gain(node, "MPC_Z_VEL_D"); + } // migrate MC_DTERM_CUTOFF -> IMU_DGYRO_CUTOFF (2020-03-12). This can be removed after the next release (current release=1.10) if (node->type == BSON_DOUBLE) { @@ -148,3 +157,12 @@ bool param_modify_on_import(bson_node_t node) return false; } + +void param_migrate_velocity_gain(bson_node_t node, const char *parameter_name) +{ + if (strcmp(parameter_name, node->name) == 0) { + strcat(node->name, "_ACC"); + node->d *= 20.0; + PX4_INFO("migrating %s (removed) -> %s: new value=%.3f", parameter_name, node->name, node->d); + } +} diff --git a/src/lib/parameters/param_translation.h b/src/lib/parameters/param_translation.h index 1d001ce09e..2da4aa1dad 100644 --- a/src/lib/parameters/param_translation.h +++ b/src/lib/parameters/param_translation.h @@ -36,3 +36,4 @@ #include "tinybson/tinybson.h" __EXPORT bool param_modify_on_import(bson_node_t node); +__EXPORT void param_migrate_velocity_gain(bson_node_t node, const char *parameter_name); diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index d3dd8b3fc2..4fb98948e8 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -37,11 +37,12 @@ #include "Takeoff.hpp" #include +#include -void Takeoff::generateInitialRampValue(const float hover_thrust, float velocity_p_gain) +void Takeoff::generateInitialRampValue(float velocity_p_gain) { velocity_p_gain = math::max(velocity_p_gain, 0.01f); - _takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain; + _takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain; } void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 55874cf2ed..6b2cc6b94f 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -68,7 +68,7 @@ public: * @param hover_thrust normalized thrsut value with which the vehicle hovers * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust */ - void generateInitialRampValue(const float hover_thrust, const float velocity_p_gain); + void generateInitialRampValue(const float velocity_p_gain); /** * Update the state for the takeoff. diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp index 6666ba7d88..c3262338ac 100644 --- a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp +++ b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp @@ -34,6 +34,7 @@ #include #include #include +#include TEST(TakeoffTest, Initialization) { @@ -46,7 +47,7 @@ TEST(TakeoffTest, RegularTakeoffRamp) Takeoff takeoff; takeoff.setSpoolupTime(1.f); takeoff.setTakeoffRampTime(2.0); - takeoff.generateInitialRampValue(.5f, 1.f); + takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); // disarmed, landed, don't want takeoff takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 520c8ad3dd..59924d5eef 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -150,12 +150,12 @@ private: // Position Control (ParamFloat) _param_mpc_xy_p, (ParamFloat) _param_mpc_z_p, - (ParamFloat) _param_mpc_xy_vel_p, - (ParamFloat) _param_mpc_xy_vel_i, - (ParamFloat) _param_mpc_xy_vel_d, - (ParamFloat) _param_mpc_z_vel_p, - (ParamFloat) _param_mpc_z_vel_i, - (ParamFloat) _param_mpc_z_vel_d, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_i_acc, + (ParamFloat) _param_mpc_xy_vel_d_acc, + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_z_vel_i_acc, + (ParamFloat) _param_mpc_z_vel_d_acc, (ParamFloat) _param_mpc_xy_vel_max, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, @@ -362,12 +362,10 @@ MulticopterPositionControl::parameters_update(bool force) } _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); - // backwards compatibility scale for velocity gains to non-acceleration based control, needs to be overcome with configuration conversion - const float hover_scale = 20.f; - _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), - _param_mpc_z_vel_p.get()) * hover_scale, - Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()) * hover_scale, - Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()) * hover_scale); + _control.setVelocityGains( + Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), + Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), + Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians! @@ -407,7 +405,7 @@ MulticopterPositionControl::parameters_update(bool force) // set trigger time for takeoff delay _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); - _takeoff.generateInitialRampValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get()); + _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); if (_wv_controller != nullptr) { _wv_controller->update_parameters(); diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 30125f4a8a..6654d1b0c5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -148,34 +148,40 @@ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); /** * Proportional gain for vertical velocity error * - * @min 0.1 - * @max 0.4 + * defined as correction acceleration in m/s^2 per m/s velocity error + * + * @min 2.0 + * @max 8.0 * @decimal 2 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f); /** * Integral gain for vertical velocity error * + * defined as correction acceleration in m/s^2 per m velocity integral + * * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. * - * @min 0.01 - * @max 0.1 + * @min 0.2 + * @max 2.0 * @decimal 3 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f); /** * Differential gain for vertical velocity error * + * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative + * * @min 0.0 - * @max 0.1 + * @max 2.0 * @decimal 3 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f); /** * Maximum vertical ascent velocity @@ -215,34 +221,39 @@ PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); /** * Proportional gain for horizontal velocity error * - * @min 0.06 - * @max 0.15 + * defined as correction acceleration in m/s^2 per m/s velocity error + * + * @min 1.2 + * @max 3.0 * @decimal 2 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.09f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); /** * Integral gain for horizontal velocity error * + * defined as correction acceleration in m/s^2 per m velocity integral * Non-zero value allows to eliminate steady state errors in the presence of disturbances like wind. * * @min 0.0 - * @max 3.0 + * @max 60.0 * @decimal 3 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); /** * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. * - * @min 0.005 - * @max 0.1 + * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.1 + * @max 2.0 * @decimal 3 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); /** * Maximum horizontal velocity in mission