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