mavlink: increase optimization to ${MAX_CUSTOM_OPT_LEVEL}

- MAX_CUSTOM_OPT_LEVEL is -O2 on boards that aren't flash constrained

ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc

@@ -1,110 +0,0 @@
-#!/bin/sh
-#
-# @name Standard VTOL
-#
-# @type Standard VTOL
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default VM_MASS 1.5
-param set-default VM_INERTIA_XX 0.03
-param set-default VM_INERTIA_YY 0.03
-param set-default VM_INERTIA_ZZ 0.05
-
-param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
-param set-default FW_PR_FF 0.2
-param set-default FW_PR_P 0.9
-param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
-param set-default FW_P_LIM_MIN -15
-param set-default FW_RR_FF 0.1
-param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.25
-param set-default FW_THR_MAX 0.6
-param set-default FW_THR_MIN 0.05
-param set-default FW_T_ALT_TC 2
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_HRATE_FF 0.5
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-param set-default FW_T_TAS_TC 2
-
-param set-default MC_ROLLRATE_P 0.3
-param set-default MC_YAW_P 1.6
-
-param set-default MIS_TAKEOFF_ALT 10
-
-param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
-param set-default NAV_ACC_RAD 5
-param set-default NAV_LOITER_RAD 80
-
-param set-default VT_FWD_THRUST_EN 4
-param set-default VT_F_TRANS_THR 0.75
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-param set-default VT_B_TRANS_DUR 8
-param set-default VT_TYPE 2
-
-param set-default CA_AIRFRAME 1
-param set-default CA_METHOD 1
-
-# VTOL Motors
-param set-default CA_ACT0_MIN 0.0
-param set-default CA_ACT1_MIN 0.0
-param set-default CA_ACT2_MIN 0.0
-param set-default CA_ACT3_MIN 0.0
-param set-default CA_ACT0_MAX 1.0
-param set-default CA_ACT1_MAX 1.0
-param set-default CA_ACT2_MAX 1.0
-param set-default CA_ACT3_MAX 1.0
-
-# Throttle
-param set-default CA_ACT4_MIN 0.0
-param set-default CA_ACT4_MAX 1.0
-
-# Ailerons/Ruddervator
-param set-default CA_ACT5_MIN -1.0
-param set-default CA_ACT5_MAX 1.0
-param set-default CA_ACT6_MIN -1.0
-param set-default CA_ACT6_MAX 1.0
-param set-default CA_ACT7_MIN -1.0
-param set-default CA_ACT7_MAX 1.0
-
-param set-default CA_MC_R0_PX 0.177
-param set-default CA_MC_R0_PY 0.177
-param set-default CA_MC_R0_CT 6.5
-param set-default CA_MC_R0_KM 0.05
-param set-default CA_MC_R1_PX -0.177
-param set-default CA_MC_R1_PY -0.177
-param set-default CA_MC_R1_CT 6.5
-param set-default CA_MC_R1_KM 0.05
-param set-default CA_MC_R2_PX 0.177
-param set-default CA_MC_R2_PY -0.177
-param set-default CA_MC_R2_CT 6.5
-param set-default CA_MC_R2_KM -0.05
-param set-default CA_MC_R3_PX -0.177
-param set-default CA_MC_R3_PY 0.177
-param set-default CA_MC_R3_CT 6.5
-param set-default CA_MC_R3_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105 # Setting pusher to Motor5 for now
-param set-default PWM_MAIN_FUNC6 201
-param set-default PWM_MAIN_FUNC7 202
-param set-default PWM_MAIN_FUNC8 203
-
-set MAV_TYPE 22
-
-set MIXER skip

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -67,7 +67,6 @@ px4_add_romfs_files(
 	1042_tiltrotor
 	1043_standard_vtol_drop
 	1043_standard_vtol_drop.post
-	1050_standard_vtol_ctrlalloc
 	1060_rover
 	1061_r1_rover
 	1062_tf-r1

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -15,22 +15,6 @@ ekf2 start &
 #                        End Estimator group selection                        #
 ###############################################################################
 
-if param compare SYS_CTRL_ALLOC 1
-then
-	#
-	# Start Control Allocator
-	#
-	control_allocator start
-
-	#
-	# Disable hover thrust estimator and prearming
-	# These features are currently incompatible with control allocation
-	#
-	# TODO: fix
-	#
-	param set MPC_USE_HTE 0
-fi
-
 airspeed_selector start
 
 vtol_att_control start

Tools/jMAVSim

@@ -1 +1 @@
-Subproject commit 0a5a8c6d9da05bee5f82faa25430213d97d396b0
+Subproject commit b23dc53d558e801b214fbcb605a061c9773105e0

Tools/sitl_gazebo

@@ -1 +1 @@
-Subproject commit d8c5f58543eb73763e829ddcca8f34149ed56285
+Subproject commit d8366bf2389eae6106d1dbfaac72ebfdf23a5d2d

cmake/px4_add_module.cmake

@@ -156,11 +156,12 @@ function(px4_add_module)
 	if(NOT DYNAMIC)
 		target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface px4_layer px4_platform systemlib)
 		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
-		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
-		px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS})
-		set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS})
 	endif()
 
+	set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
+	px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS})
+	set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS})
+
 	# set defaults if not set
 	set(MAIN_DEFAULT MAIN-NOTFOUND)
 	set(STACK_MAIN_DEFAULT 2048)

platforms/posix/cmake/sitl_target.cmake

@@ -144,7 +144,6 @@ set(models
 	solo
 	standard_vtol
 	standard_vtol_drop
-	standard_vtol_ctrlalloc
 	tailsitter
 	techpod
 	tiltrotor

src/drivers/distance_sensor/ulanding_radar/module.yaml

@@ -1,6 +1,6 @@
 module_name: uLanding Radar
 serial_config:
-    - command: ulanding_radar start ${SERIAL_DEV}
+    - command: ulanding_radar start -d ${SERIAL_DEV}
       port_config_param:
         name: SENS_ULAND_CFG
         group: Sensors

src/modules/commander/Commander.cpp

@@ -3912,7 +3912,7 @@ void Commander::estimator_check()
 
 		if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
-			if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+			if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 				_nav_test_failed = false;
 				_nav_test_passed = false;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -44,7 +44,6 @@
 #include <ControlAllocation/ControlAllocation.hpp>
 
 #include <matrix/matrix/math.hpp>
-#include <mixer_module/output_functions.hpp>
 #include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ActuatorEffectiveness
@@ -105,11 +104,6 @@ public:
 	 */
 	virtual int numActuators() const = 0;
 
-	/**
-	 * Get the function of an actuator
-	 */
-	virtual int actuatorFunction(uint8_t idx) const = 0;
-
 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -78,8 +78,6 @@ public:
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
 
 	int numActuators() const override { return _num_actuators; }
-
-	int actuatorFunction(uint8_t idx) const override { return (int)OutputFunction::Motor1 + idx; }
 private:
 	bool _updated{true};
 	int _num_actuators{0};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -58,15 +58,7 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
-	int numActuators() const override { return 8; }
-
-	int actuatorFunction(uint8_t idx) const override { return (int)_actuator_functions[idx]; }
-
+	int numActuators() const override { return 7; }
 protected:
-	OutputFunction _actuator_functions[NUM_ACTUATORS] = {
-		OutputFunction::Motor1, OutputFunction::Motor2, OutputFunction::Motor3, OutputFunction::Motor4,
-		OutputFunction::Motor5, OutputFunction::Servo1, OutputFunction::Servo2, OutputFunction::Servo3
-	};
-
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -59,8 +59,6 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	int numActuators() const override { return 10; }
-
-	int actuatorFunction(uint8_t idx) const { return (int)OutputFunction::Motor1 + idx; }; /// TODO: This is wrong.  Someone familiar with the tiltrotor should fix this.
 protected:
 	bool _updated{true};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -436,35 +436,21 @@ ControlAllocator::publish_control_allocator_status()
 void
 ControlAllocator::publish_legacy_actuator_controls()
 {
-	const hrt_abstime now = hrt_absolute_time();
-
-	actuator_motors_s actuator_motors {};
-	actuator_motors.timestamp = now;
+	actuator_motors_s actuator_motors;
+	actuator_motors.timestamp = hrt_absolute_time();
 	actuator_motors.timestamp_sample = _timestamp_sample;
 
-	actuator_servos_s actuator_servos {};
-	actuator_servos.timestamp = now;
-	actuator_servos.timestamp_sample = _timestamp_sample;
-
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
 				actuator_sp);
 
-	for (size_t i = 0; i < NUM_ACTUATORS; i++) {
-		const int func = _actuator_effectiveness->actuatorFunction(i);
-		const float control = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
-
-		if (func >= (int)OutputFunction::Motor1 && func <= (int)OutputFunction::MotorMax) {
-			actuator_motors.control[func - (int)OutputFunction::Motor1] = control;
-		}
-
-		if (func >= (int)OutputFunction::Servo1 && func <= (int)OutputFunction::ServoMax) {
-			actuator_servos.control[func - (int)OutputFunction::Servo1] = control;
-		}
+	for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
+		actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
 	}
 
 	_actuator_motors_pub.publish(actuator_motors);
-	_actuator_servos_pub.publish(actuator_servos);
+
+	// TODO: servos
 }
 
 int ControlAllocator::task_spawn(int argc, char *argv[])

src/modules/mavlink/CMakeLists.txt

@@ -37,6 +37,7 @@ px4_add_module(
 	MODULE modules__mavlink
 	MAIN mavlink
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 		-Wno-cast-align # TODO: fix and enable
 		-Wno-address-of-packed-member # TODO: fix in c_library_v2
 		-Wno-enum-compare # ROTATION <-> MAV_SENSOR_ROTATION