Stable v1.9.1 fixes

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

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 <[email protected]>
+#
+
+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

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

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

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
 };
 
 

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));

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;

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

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -48,6 +48,9 @@
  */
 #include "vtol_att_control_main.h"
 #include <systemlib/mavlink_log.h>
+#include <matrix/matrix/math.hpp>
+
+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 ||