Use bc to support floating point PX4_SIM_SPEED_FACTOR

Fixes errors like:
ERROR [logger] No definition for topic char[10] key;uint8_t[2] _padding0; found
when the Debug logging profile is selected.

CMakeLists.txt

@@ -415,6 +415,9 @@ endif()
 
 # optionally enable cmake testing (supported only on posix)
 option(CMAKE_TESTING "Configure test targets" OFF)
+if (${PX4_CONFIG} STREQUAL "px4_sitl_test")
+	set(CMAKE_TESTING ON)
+endif()
 if(CMAKE_TESTING)
 	include(CTest) # sets BUILD_TESTING variable
 endif()

Makefile

@@ -344,7 +344,6 @@ format:
 .PHONY: rostest python_coverage
 
 tests:
-	$(eval CMAKE_ARGS += -DCMAKE_TESTING=ON)
 	$(eval CMAKE_ARGS += -DCONFIG=px4_sitl_test)
 	$(eval CMAKE_ARGS += -DTESTFILTER=$(TESTFILTER))
 	$(eval ARGS += test_results)

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -162,15 +162,15 @@ fi
 
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then
-	COM_DL_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
+	COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
 	echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
 	param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
 
-	COM_RC_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 1))
+	COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1" | bc)
 	echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
 	param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
 
-	COM_OF_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
+	COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
 	echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
 	param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
 fi

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 <lorenz@px4.io>
+#
+
+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/init/rc.board_sensors

@@ -20,6 +20,7 @@ ist8310 -C -b 1 start
 ist8310 -C -b 2 start
 hmc5883 -C -T -X start
 qmc5883 -X start
+lis3mdl -X start
 
 # Possible internal compass
 ist8310 -C -b 5 start

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/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_generateLandSetpoints()
 	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
 	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
 	// set constraints
-	_constraints.tilt = _param_mpc_tiltmax_lnd.get();
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
 	_constraints.speed_down = _param_mpc_land_speed.get();
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -132,7 +132,7 @@ void FlightTaskAutoMapper2::_prepareLandSetpoints()
 	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
 
 	// set constraints
-	_constraints.tilt = _param_mpc_tiltmax_lnd.get();
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 

src/modules/ekf2/ekf2_main.cpp

@@ -966,8 +966,8 @@ void Ekf2::run()
 					const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq);
 					const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq);
 
-					const float pstatic_err = 0.5f * airdata.rho *
-								  (K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_param_ekf2_pcoef_z.get() * z_v2);
+					const float pstatic_err = 0.5f * airdata.rho * (
+									  K_pstatic_coef_x * x_v2 + K_pstatic_coef_y * y_v2 + _param_ekf2_pcoef_z.get() * z_v2);
 
 					// correct baro measurement using pressure error estimate and assuming sea level gravity
 					balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);

src/modules/logger/logger.cpp

@@ -1798,7 +1798,8 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats
 				strcmp(type_name, "uint64_t") != 0 &&
 				strcmp(type_name, "float") != 0 &&
 				strcmp(type_name, "double") != 0 &&
-				strcmp(type_name, "bool") != 0) {
+				strcmp(type_name, "bool") != 0 &&
+				strcmp(type_name, "char") != 0) {
 
 			// find orb meta for type
 			const orb_metadata *const*topics = orb_get_topics();

src/modules/mavlink/mavlink_main.cpp

@@ -1704,7 +1704,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
 		configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
 		configure_stream_local("RC_CHANNELS", 20.0f);
-		configure_stream_local("SCALED_IMU", 50.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
 		configure_stream_local("SYS_STATUS", 5.0f);
 		configure_stream_local("SYSTEM_TIME", 1.0f);
@@ -1792,6 +1791,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("PING", 1.0f);
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
 		configure_stream_local("RC_CHANNELS", 10.0f);
+		configure_stream_local("SCALED_IMU", 25.0f);
+		configure_stream_local("SCALED_IMU2", 25.0f);
+		configure_stream_local("SCALED_IMU3", 25.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);

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/mc_pos_control/PositionControl.cpp

@@ -331,9 +331,11 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
 	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
 	// constraints, then just use global constraints for the limits.
 
+	const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()));
+
 	if (!PX4_ISFINITE(constraints.tilt)
-	    || !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) {
-		_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get());
+	    || !(constraints.tilt < tilt_max_radians)) {
+		_constraints.tilt = tilt_max_radians;
 	}
 
 	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) {
@@ -352,8 +354,4 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
 void PositionControl::updateParams()
 {
 	ModuleParams::updateParams();
-
-	// Tilt needs to be in radians
-	_param_mpc_tiltmax_air.set(math::radians(_param_mpc_tiltmax_air.get()));
-	_param_mpc_man_tilt_max.set(math::radians(_param_mpc_man_tilt_max.get()));
 }

src/modules/mc_pos_control/PositionControl.hpp

@@ -232,9 +232,9 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
 		(ParamFloat<px4::params::MPC_TILTMAX_AIR>)
-		_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in radians
+		_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees
 		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
-		_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in radians
+		_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees
 		(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
 		(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
 		(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,

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