Added param system command to voxl2 slpi build

- needs to be called every iteration

boards/modalai/voxl2-slpi/default.px4board

@@ -10,7 +10,6 @@ CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
-CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_EKF2=y
@@ -28,4 +27,5 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_ORB_COMMUNICATOR=y

boards/modalai/voxl2-slpi/src/CMakeLists.txt

@@ -49,6 +49,6 @@ add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
-# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)

boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp

@@ -989,10 +989,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
 
 	gps.device_id = device_id.devid;
 
-	gps.lat = hil_gps.lat;
-	gps.lon = hil_gps.lon;
-	gps.alt = hil_gps.alt;
-	gps.alt_ellipsoid = hil_gps.alt;
+	gps.latitude_deg = hil_gps.lat;
+	gps.longitude_deg = hil_gps.lon;
+	gps.altitude_msl_m = hil_gps.alt;
+	gps.altitude_ellipsoid_m = hil_gps.alt;
 
 	gps.s_variance_m_s = 0.25f;
 	gps.c_variance_rad = 0.5f;

boards/px4/fmu-v5/rover.px4board

@@ -18,4 +18,4 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

boards/px4/fmu-v6c/rover.px4board

@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

boards/px4/fmu-v6u/rover.px4board

@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

boards/px4/fmu-v6x/rover.px4board

@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -8,4 +8,4 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

msg/CMakeLists.txt

@@ -173,6 +173,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	Rpm.msg
+	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
 	SensorAccel.msg

msg/RtlStatus.msg

@@ -0,0 +1,15 @@
+uint64 timestamp                      # time since system start (microseconds)
+
+uint32 safe_points_id 		      # unique ID of active set of safe_point_items
+bool is_evaluation_pending 	      # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
+
+bool has_vtol_approach 		      # flag if approaches are defined for current RTL_TYPE parameter setting
+
+uint8 rtl_type	      		      # Type of RTL chosen
+uint8 safe_point_index 		      # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
+
+uint8 RTL_STATUS_TYPE_NONE=0       # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
+uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
+uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 	# RTL type is following the mission from closest point to mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position

platforms/common/pab_manifest.c

@@ -48,6 +48,8 @@
 
 #include <board_config.h>
 
+#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
+
 #include <inttypes.h>
 #include <stdbool.h>
 #include <syslog.h>
@@ -57,7 +59,6 @@
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
 
 typedef struct {
 	hw_base_id_t             hw_base_id; /* The ID of the Base */

platforms/posix/src/px4/common/px4_init.cpp

@@ -40,15 +40,34 @@
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 #include <uORB/uORB.h>
 
+#if defined(CONFIG_MODULES_MUORB_APPS)
+extern "C" { int muorb_init(); }
+#endif
+
 int px4_platform_init(void)
 {
 	hrt_init();
 
 	px4::WorkQueueManagerStart();
 
+// MUORB has slightly different startup requirements
+#if defined(CONFIG_MODULES_MUORB_APPS)
+	//Put sleeper in here to allow wq to finish initializing before param_init is called
+	usleep(10000);
+
+	uorb_start();
+
+	muorb_init();
+
+	// Give muorb some time to setup the DSP
+	usleep(100000);
+
+	param_init();
+#else
 	param_init();
 
 	uorb_start();
+#endif
 
 	px4_log_initialize();
 

src/drivers/batt_smbus/batt_smbus.cpp

@@ -392,6 +392,12 @@ int BATT_SMBUS::get_startup_info()
 	uint16_t state_of_health;
 	ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);
 
+	/* ManufacturerAccess dummy command to init the ManufacturerBlockAccess routine
+	in the BQ40Zx0 and avoid timeout during LifetimeDataFlush.
+	test Sleep > 20 ms to give time to init the ManufacturerBlockAccess routine*/
+	ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, BATT_SMBUS_DEVICE_TYPE);
+	px4_usleep(30_ms);
+
 	if (!ret) {
 		_serial_number = serial_num;
 		_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);

src/drivers/batt_smbus/batt_smbus.h

@@ -107,6 +107,7 @@ using namespace time_literals;
 
 #define BATT_SMBUS_SECURITY_KEYS                        0x0035
 
+#define BATT_SMBUS_DEVICE_TYPE                          0x0001
 #define BATT_SMBUS_LIFETIME_FLUSH                       0x002E
 #define BATT_SMBUS_LIFETIME_BLOCK_ONE                   0x0060
 #define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS        0x4938

src/drivers/voxl2_io/CMakeLists.txt

@@ -45,6 +45,7 @@ px4_add_module(
 		voxl2_io.cpp
 		voxl2_io.hpp
 	DEPENDS
+		rc
 		px4_work_queue
 		mixer_module
 	MODULE_CONFIG

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -97,6 +97,7 @@ PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2);
  * @decimal 2
  * @reboot_required true
  * @group Airspeed Validator
+ * @volatile
  */
 PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f);
 
@@ -110,6 +111,7 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f);
  * @decimal 2
  * @reboot_required true
  * @group Airspeed Validator
+ * @volatile
  */
 PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f);
 
@@ -123,6 +125,7 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f);
  * @decimal 2
  * @reboot_required true
  * @group Airspeed Validator
+ * @volatile
  */
 PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f);
 

src/modules/ekf2/EKF/ekf.h

@@ -477,7 +477,7 @@ public:
 
 		for (unsigned row = 0; row < State::size; row++) {
 			for (unsigned col = 0; col < State::size; col++) {
-				// Instad of literally computing KHP, use an equvalent
+				// Instead of literally computing KHP, use an equivalent
 				// equation involving less mathematical operations
 				KHP(row, col) = KS(row) * K(col);
 			}

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -684,7 +684,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status_u soln_status{};
 	// TODO: Is this accurate enough?
-	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
+	soln_status.flags.attitude = attitude_valid();
 	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
 	soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);

src/modules/ekf2/EKF/ev_control.cpp

@@ -41,6 +41,7 @@
 void Ekf::controlExternalVisionFusion()
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
+	_ev_hgt_b_est.predict(_dt_ekf_avg);
 
 	// Check for new external vision data
 	extVisionSample ev_sample;

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -45,7 +45,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 
 	HeightBiasEstimator &bias_est = _ev_hgt_b_est;
 
-	bias_est.predict(_dt_ekf_avg);
+	// bias_est.predict(_dt_ekf_avg) called by controlExternalVisionFusion()
 
 	// correct position for offset relative to IMU
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;

src/modules/ekf2/EKF/gps_control.cpp

@@ -80,8 +80,11 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			}
 		}
 
+		if (_pos_ref.isInitialized()) {
+			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
+		}
+
 		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
-		updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 
 	} else if (_control_status.flags.gps) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {

src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt

@@ -40,3 +40,5 @@ px4_add_library(FlightTaskUtility
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)
 target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility)

src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp

@@ -40,7 +40,18 @@ using namespace matrix;
 
 StickTiltXY::StickTiltXY(ModuleParams *parent) :
 	ModuleParams(parent)
-{}
+{
+	updateParams();
+}
+
+void StickTiltXY::updateParams()
+{
+	ModuleParams::updateParams();
+	// Consider maximum tilt but only between [0.02,3]g sideways acceleration -> ~[1,71]° tilt
+	// Constrain tilt already because tanf(90+°) will give negative result
+	const float maximum_tilt = math::radians(math::constrain(_param_mpc_man_tilt_max.get(), 0.f, 89.f));
+	_maximum_acceleration = math::constrain(tanf(maximum_tilt), .02f, 3.f) * CONSTANTS_ONE_G;
+}
 
 Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw,
 		const float yaw_setpoint)
@@ -49,5 +60,5 @@ Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const flo
 	_man_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
 	stick_xy = _man_input_filter.update(stick_xy);
 	Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint);
-	return stick_xy * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
+	return stick_xy * _maximum_acceleration;
 }

src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp

@@ -63,6 +63,9 @@ public:
 	matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw,
 			const float yaw_setpoint);
 private:
+	void updateParams() override;
+
+	float _maximum_acceleration{0.f};
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
 
 	DEFINE_PARAMETERS(

src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "StickTiltXY.hpp"
+
+#include <geo/geo.h>
+
+using namespace matrix;
+
+TEST(StickTiltXYTest, AllZeroCase)
+{
+	StickTiltXY stick_tilt_xy{nullptr};
+	Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 0.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f());
+}
+
+TEST(StickTiltXYTest, NormalRollPitchCases)
+{
+	// Disable autosaving parameters to avoid busy loop in param_set()
+	param_control_autosave(false);
+
+	float value = 45.f;
+	param_set(param_find("MPC_MAN_TILT_MAX"), &value);
+
+	StickTiltXY stick_tilt_xy{nullptr};
+	// Pitch
+	Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G, 0.f));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(.5f, 0.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / 2.f, 0.f));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-.5f, 0.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / 2.f, 0.f));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 0.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G, 0.f));
+	// Roll
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, 1.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, .5f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G / 2.f));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -.5f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G / 2.f));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -1.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G));
+	// Roll & Pitch
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 1.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, -1.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 1.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F));
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, -1.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F));
+}
+
+TEST(StickTiltXYTest, 90degreeCase)
+{
+	// Disable autosaving parameters to avoid busy loop in param_set()
+	param_control_autosave(false);
+
+	float value = 90.f;
+	param_set(param_find("MPC_MAN_TILT_MAX"), &value);
+
+	StickTiltXY stick_tilt_xy{nullptr};
+	// Pitch
+	// Zero input leads to zero output
+	Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f());
+	// Maximum input leads to the maximum of 3g sideways acceleration
+	acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f);
+	EXPECT_EQ(acc_xy, Vector2f(3.f * CONSTANTS_ONE_G, 0.f));
+}

src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp

@@ -92,7 +92,7 @@ public:
 
 private:
 	bool _input_available{false};
-	matrix::Vector4f _positions; ///< unmodified manual stick inputs
+	matrix::Vector4f _positions; ///< unmodified manual stick inputs that usually move vehicle in x, y, z and yaw direction
 	matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function
 
 	matrix::Vector<float, 6> _aux_positions;

src/modules/logger/logged_topics.cpp

@@ -102,6 +102,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
 	add_topic("rtl_time_estimate", 1000);
+	add_optional_topic("rtl_status", 5000);
 	add_optional_topic("sensor_airflow", 100);
 	add_topic("sensor_combined");
 	add_optional_topic("sensor_correction");

src/modules/mc_pos_control/multicopter_stabilized_mode_params.c

@@ -35,8 +35,8 @@
  * Maximal tilt angle in Stabilized or Altitude mode
  *
  * @unit deg
- * @min 0
- * @max 90
+ * @min 1
+ * @max 70
  * @decimal 0
  * @increment 1
  * @group Multicopter Position Control

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -275,10 +275,10 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
 		uORB::Manager::get_instance()->set_uorb_communicator(
 			uORB::ProtobufChannel::GetInstance());
 
-		param_init();
-
 		px4::WorkQueueManagerStart();
 
+		param_init();
+
 		uORB::ProtobufChannel::GetInstance()->RegisterSendHandler(muorb_func_ptrs.topic_data_func_ptr);
 
 		// Configure the I2C driver function pointers

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -125,6 +125,12 @@ void FeasibilityChecker::updateData()
 		_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
 	}
 
+	if (_rtl_status_sub.updated()) {
+		rtl_status_s rtl_status = {};
+		_rtl_status_sub.copy(&rtl_status);
+		_has_vtol_approach = rtl_status.has_vtol_approach;
+	}
+
 	param_t handle = param_find("FW_LND_ANG");
 
 	if (handle != PARAM_INVALID) {
@@ -577,17 +583,22 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 		break;
 
 	case 4:
-		result = _has_takeoff == _landing_valid;
+		result = hasMissionBothOrNeitherTakeoffAndLanding();
 
-		if (!result && (_has_takeoff)) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
-			events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Add Landing item or remove Takeoff");
+		break;
 
-		} else if (!result && (_landing_valid)) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
-			events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Add Takeoff item or remove Landing");
+	case 5:
+		if (!_is_landed && !_has_vtol_approach) {
+			result = _landing_valid;
+
+			if (!result) {
+				mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required.");
+				events::send(events::ID("feasibility_mis_in_air_landing_req"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: Landing waypoint/pattern required");
+			}
+
+		} else {
+			result = hasMissionBothOrNeitherTakeoffAndLanding();
 		}
 
 		break;
@@ -600,6 +611,23 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 	return result;
 }
 
+bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding()
+{
+	bool result{_has_takeoff == _landing_valid};
+
+	if (!result && (_has_takeoff)) {
+		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
+		events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: Add Landing item or remove Takeoff");
+
+	} else if (!result && (_landing_valid)) {
+		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
+		events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: Add Takeoff item or remove Landing");
+	}
+
+	return result;
+}
 
 bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
 {

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -36,6 +36,7 @@
 #include "../navigation.h"
 #include <mathlib/mathlib.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/rtl_status.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
@@ -97,6 +98,7 @@ private:
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)};
 
 	// parameters
 	float _param_fw_lnd_ang{0.f};
@@ -106,6 +108,7 @@ private:
 
 	bool _is_landed{false};
 	float _home_alt_msl{NAN};
+	bool _has_vtol_approach{false};
 	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	VehicleType _vehicle_type{VehicleType::RotaryWing};
@@ -247,4 +250,8 @@ private:
 	 * @return False if the check failed.
 	*/
 	void doMulticopterChecks(mission_item_s &mission_item, const int current_index);
+
+	// Helper functions
+
+	bool hasMissionBothOrNeitherTakeoffAndLanding();
 };

src/modules/navigator/land.cpp

@@ -83,7 +83,7 @@ Land::on_active()
 
 		// create a virtual wp 1m in front of the vehicle to track during the backtransition
 		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-						   _navigator->get_position_setpoint_triplet()->current.yaw, 1.f,
+						   _navigator->get_local_position()->heading, 1.f,
 						   &pos_sp_triplet->current.lat, &pos_sp_triplet->current.lon);
 
 		_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/mission_params.c

@@ -68,6 +68,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
  * @value 2 Require a landing
  * @value 3 Require a takeoff and a landing
  * @value 4 Require both a takeoff and a landing, or neither
+ * @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);

src/modules/navigator/navigator_main.cpp

@@ -208,7 +208,7 @@ void Navigator::run()
 
 			if (mission.safe_points_id != safe_points_id) {
 				safe_points_id = mission.safe_points_id;
-				_rtl.updateSafePoints();
+				_rtl.updateSafePoints(safe_points_id);
 			}
 		}
 

src/modules/navigator/rtl.cpp

@@ -294,12 +294,14 @@ void RTL::setRtlTypeAndDestination()
 
 	init_rtl_mission_type();
 
+	uint8_t safe_point_index{0U};
+
 	if (_param_rtl_type.get() != 2) {
 		// check the closest allowed destination.
 		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
 		PositionYawSetpoint rtl_position;
 		float rtl_alt;
-		findRtlDestination(destination_type, rtl_position, rtl_alt);
+		findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
 
 		switch (destination_type) {
 		case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@@ -331,9 +333,29 @@ void RTL::setRtlTypeAndDestination()
 			break;
 		}
 	}
+
+	// Publish rtl status
+	_rtl_status_pub.get().timestamp = hrt_absolute_time();
+	_rtl_status_pub.get().safe_points_id = _safe_points_id;
+	_rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait;
+	_rtl_status_pub.get().has_vtol_approach = false;
+
+	if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) {
+		_rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
+
+	} else if (_param_rtl_type.get() == 1) {
+		_rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach;
+	}
+
+	_rtl_status_pub.get().rtl_type = static_cast<uint8_t>(_rtl_type);
+	_rtl_status_pub.get().safe_point_index = safe_point_index;
+
+	_rtl_status_pub.update();
+
 }
 
-void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt)
+void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
+			     uint8_t &safe_point_index)
 {
 	// set destination to home per default, then check if other valid landing spot is closer
 	rtl_position.alt = _home_pos_sub.get().alt;
@@ -352,8 +374,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 	float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
 	float min_dist;
 
+	_home_has_land_approach = hasVtolLandApproach(rtl_position);
+
 	if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1)
-			&& !hasVtolLandApproach(rtl_position))) {
+			&& !_home_has_land_approach)) {
 		// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home.
 		min_dist = FLT_MAX;
 
@@ -394,6 +418,8 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 
 	if (_safe_points_updated) {
 
+		_one_rally_point_has_land_approach = false;
+
 		for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
 			mission_item_s mission_safe_point;
 
@@ -416,11 +442,16 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 				PositionYawSetpoint safepoint_position;
 				setSafepointAsDestination(safepoint_position, mission_safe_point);
 
+				bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
+
+				_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
+
 				if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0)
-						|| hasVtolLandApproach(safepoint_position))) {
+						|| current_safe_point_has_approaches)) {
 					min_dist = dist;
 					rtl_position = safepoint_position;
 					destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
+					safe_point_index = current_seq;
 				}
 			}
 		}

src/modules/navigator/rtl.h

@@ -58,6 +58,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rtl_status.h>
 #include <uORB/topics/rtl_time_estimate.h>
 
 class Navigator;
@@ -86,7 +87,7 @@ public:
 
 	void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
 
-	void updateSafePoints() { _initiate_safe_points_updated = true; }
+	void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }
 
 private:
 	enum class DestinationType {
@@ -109,7 +110,8 @@ private:
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt);
+	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
+				uint8_t &safe_point_index);
 
 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
@@ -188,6 +190,9 @@ private:
 
 	RtlType _rtl_type{RtlType::RTL_DIRECT};
 
+	bool _home_has_land_approach;			///< Flag if the home position has a land approach defined
+	bool _one_rally_point_has_land_approach;	///< Flag if a rally point has a land approach defined
+
 	DatamanState _dataman_state{DatamanState::UpdateRequestWait};
 	DatamanState _error_state{DatamanState::UpdateRequestWait};
 	uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated
@@ -197,6 +202,7 @@ private:
 	bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
 	mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
 	uint32_t _mission_id = 0u;
+	uint32_t _safe_points_id = 0u;
 
 	mission_stats_entry_s _stats;
 
@@ -222,4 +228,5 @@ private:
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
 
 	uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
+	uORB::PublicationData<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};
 };