diff --git a/ROMFS/px4fmu_common/init.d/airframes/4014_s500 b/ROMFS/px4fmu_common/init.d/airframes/4014_s500 index 40e9f59945df..48c018c0b31b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4014_s500 +++ b/ROMFS/px4fmu_common/init.d/airframes/4014_s500 @@ -1,6 +1,6 @@ #!/bin/sh # -# @name S500 +# @name S500 Generic # # @type Quadrotor x # @class Copter @@ -15,12 +15,10 @@ set PWM_OUT 1234 if [ $AUTOCNF = yes ] then - param set MC_ROLL_P 6.5 param set MC_ROLLRATE_P 0.18 - param set MC_ROLLRATE_I 0.15 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 6.5 param set MC_PITCHRATE_P 0.18 + param set MC_ROLLRATE_I 0.15 param set MC_PITCHRATE_I 0.15 + param set MC_ROLLRATE_D 0.003 param set MC_PITCHRATE_D 0.003 fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 b/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 new file mode 100644 index 000000000000..fd2c87d63151 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 @@ -0,0 +1,26 @@ +#!/bin/sh +# +# @name Holybro S500 +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Lorenz Meier +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER quad_x +set PWM_OUT 1234 + +if [ $AUTOCNF = yes ] +then + param set IMU_GYRO_CUTOFF 80 + param set MC_DTERM_CUTOFF 40 + param set MC_ROLLRATE_P 0.14 + param set MC_PITCHRATE_P 0.14 + param set MC_ROLLRATE_I 0.3 + param set MC_PITCHRATE_I 0.3 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCHRATE_D 0.004 +fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 49c0fff59d30..9d12291bddab 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -69,6 +69,7 @@ px4_add_romfs_files( 4012_quad_x_can 4013_bebop 4014_s500 + 4015_holybro_s500 4020_hk_micro_pcb 4030_3dr_solo 4031_3dr_quad diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8d2deb77bd73..89405f9f68b8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -259,7 +259,7 @@ else then # Check for the mini using build with px4io fw file # but not a px4IO - if ver hwtypecmp V540 + if ver hwtypecmp V540 V560 then param set SYS_USE_IO 0 else diff --git a/boards/px4/fmu-v5/src/manifest.c b/boards/px4/fmu-v5/src/manifest.c index bcc43c59716b..c733fe971e26 100644 --- a/boards/px4/fmu-v5/src/manifest.c +++ b/boards/px4/fmu-v5/src/manifest.c @@ -90,7 +90,9 @@ static const px4_hw_mft_item_t hw_mft_list_v0540[] = { static px4_hw_mft_list_entry_t mft_lists[] = { {0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, {0x0105, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5 R:5 V:1 + {0x0500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:0 V:5 {0x0400, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, + {0x0600, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // Alias for CUAV V5nano R:0 V:6 }; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 51b4b3aee2f7..c117d6b6681c 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -178,10 +178,12 @@ bool FlightTaskAuto::_evaluateTriplets() } else { tmp_target(0) = _lock_position_xy(0); tmp_target(1) = _lock_position_xy(1); - _lock_position_xy.setAll(NAN); } } else { + // reset locked position if current lon and lat are valid + _lock_position_xy.setAll(NAN); + // Convert from global to local frame. map_projection_project(&_reference_position, _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &tmp_target(0), &tmp_target(1)); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 295ae8729643..06a256ce1fee 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -521,7 +521,7 @@ MulticopterAttitudeControl::control_attitude() { vehicle_attitude_setpoint_poll(); - // reinitialize the setpoint while not armed to make sure no value from the last flight is still kept + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept if (!_v_control_mode.flag_armed) { Quatf().copyTo(_v_att_sp.q_d); Vector3f().copyTo(_v_att_sp.thrust_body); @@ -644,8 +644,17 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) } + // I term factor: reduce the I gain with increasing rate error. + // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint + // change (noticeable in a bounce-back effect after a flip). + // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should: + // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect), + // and up to 200 deg error leads to <25% reduction of I. + float i_factor = rates_err(i) / math::radians(400.f); + i_factor = math::max(0.0f, 1.f - i_factor * i_factor); + // Perform the integration using a first order method and do not propagate the result if out of range or invalid - float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt; + float rate_i = _rates_int(i) + i_factor * rates_i_scaled(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) { _rates_int(i) = rate_i; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index bfba1ea68e38..d6049f0d8e8e 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); * @increment 0.01 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); /** * Roll rate integrator limit @@ -151,7 +151,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); * @increment 0.01 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); /** * Pitch rate integrator limit diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5e7fd840cf26..c663ec687829 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -48,6 +48,9 @@ */ #include "vtol_att_control_main.h" #include +#include + +using namespace matrix; namespace VTOL_att_control { @@ -701,6 +704,14 @@ void VtolAttitudeControl::task_main() _vtol_type->fill_actuator_outputs(); } + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_mc_virtual_att_sp.q_d); + Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + /* Only publish if the proper mode(s) are enabled */ if (_v_control_mode.flag_control_attitude_enabled || _v_control_mode.flag_control_rates_enabled ||