FixedWingModeManager: remove IDLE handling that moved in rebase

FixedWingModeManager: give idle throttle if landed
- except in takeoff where that is nonsensical - the other control_auto* and the two control_manual* functions already have such logic
2026-06-10 09:30:04 +08:00 · 2026-01-14 14:46:12 +01:00 · 2026-01-14 13:59:45 +01:00 · 2026-01-14 13:59:45 +01:00 · 2026-01-14 13:59:45 +01:00 · 2026-01-14 13:59:45 +01:00
44 changed files with 571 additions and 281 deletions
@@ -237,6 +237,12 @@ then
 	tla2528 start -X
 fi

+# Start TMP102 temperature sensor
+if param compare SENS_EN_TMP102 1
+then
+	tmp102 start -X
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then
@@ -121,21 +121,22 @@ PX4 and the receiver may also need to be configured in order to _detect RC loss_

 ![Safety - RC Loss (QGC)](../../assets/qgc/setup/safety/safety_rc_loss.png)

-The QGCroundControl Safety UI allows you to set the [failsafe action](#failsafe-actions) and [RC Loss timeout](#COM_RC_LOSS_T).
-Users that want to disable the RC loss failsafe in specific automatic modes (mission, hold, offboard) can do so using the parameter [COM_RCL_EXCEPT](#COM_RCL_EXCEPT).
+The QGCroundControl Safety UI allows you to set the [failsafe action](#failsafe-actions) and [manual control loss timeout](#COM_RC_LOSS_T).
+Users that want to disable this failsafe in specific modes can do so using the parameter [COM_RCL_EXCEPT](#COM_RCL_EXCEPT).

 Additional (and underlying) parameter settings are shown below.

 | Parameter                                                                                             | Setting                     | Description                                                                                                                                                                                                                                                                                                                                                                                              |
 | ----------------------------------------------------------------------------------------------------- | --------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| <a id="COM_RC_LOSS_T"></a>[COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T)    | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the old manual control setpoint until the timeout triggers.                                                                                                                                              |
+| <a id="COM_RC_LOSS_T"></a>[COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T)    | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the last known stick position until the timeout triggers.                                                                                                                                              |
 | <a id="COM_FAIL_ACT_T"></a>[COM_FAIL_ACT_T](../advanced_config/parameter_reference.md#COM_FAIL_ACT_T) | Failsafe Reaction Delay     | Delay in seconds between failsafe condition being triggered (`COM_RC_LOSS_T`) and failsafe action (RTL, Land, Hold). In this state the vehicle waits in hold mode for the manual control source to reconnect. This might be set longer for long-range flights so that intermittent connection loss doesn't immediately invoke the failsafe. It can be to zero so that the failsafe triggers immediately. |
 | <a id="NAV_RCL_ACT"></a>[NAV_RCL_ACT](../advanced_config/parameter_reference.md#NAV_RCL_ACT)          | Failsafe Action             | Disabled, Loiter, Return, Land, Disarm, Terminate.                                                                                                                                                                                                                                                                                                                                                       |
-| <a id="COM_RCL_EXCEPT"></a>[COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions          | Set the modes in which manual control loss is ignored: Mission, Hold, Offboard.                                                                                                                                                                                                                                                                                                                          |
+| <a id="COM_RCL_EXCEPT"></a>[COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions          | Set modes in which manual control loss is ignored.                                                                                                                                                                                                                                                                                                                        |

 ## Data Link Loss Failsafe

-The Data Link Loss failsafe is triggered if a telemetry link (connection to ground station) is lost.
+The Data Link Loss failsafe is triggered if the connection to the last MAVLink ground station like QGroundControl is lost.
+Users that want to disable this failsafe in specific modes can do so using the parameter [COM_DLL_EXCEPT](#COM_DLL_EXCEPT).

 ![Safety - Data Link Loss (QGC)](../../assets/qgc/setup/safety/safety_data_link_loss.png)

@@ -145,12 +146,7 @@ The settings and underlying parameters are shown below.
 | ---------------------- | ------------------------------------------------------------------------ | --------------------------------------------------------------------------------- |
 | Data Link Loss Timeout | [COM_DL_LOSS_T](../advanced_config/parameter_reference.md#COM_DL_LOSS_T) | Amount of time after losing the data connection before the failsafe will trigger. |
 | Failsafe Action        | [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT)     | Disabled, Hold mode, Return mode, Land mode, Disarm, Terminate.                   |
-
-The following settings also apply, but are not displayed in the QGC UI.
-
-| Setting                                                     | Parameter                                                                  | Description                                          |
-| ----------------------------------------------------------- | -------------------------------------------------------------------------- | ---------------------------------------------------- |
-| <a id="COM_DLL_EXCEPT"></a>Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes where DL loss will not trigger a failsafe. |
+| <a id="COM_DLL_EXCEPT"></a>Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes in which data link loss is ignored. |

 ## Geofence Failsafe

@@ -7,7 +7,6 @@ uint8 SETPOINT_TYPE_VELOCITY=1	# velocity setpoint
 uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
 uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
-uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)

 uint8 LOITER_TYPE_ORBIT=0 	# Circular pattern
 uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8
@@ -267,6 +267,7 @@


 #define DRV_TEMP_DEVTYPE_MCP9808	0xEE
+#define DRV_TEMP_DEVTYPE_TMP102		0xF0

 #define DRV_DEVTYPE_UNUSED		0xff

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2026 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__temperature_sensor__tmp102
+	MAIN tmp102
+	COMPILE_FLAGS
+	SRCS
+		tmp102_main.cpp
+		tmp102.cpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		px4_work_queue
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_TEMPERATURE_SENSOR_TMP102
+	bool "TMP102 temperature sensor"
+	default n
+	---help---
+		Enable support for the TMP102 temperature sensor
@@ -0,0 +1,15 @@
+__max_num_config_instances: &max_num_config_instances 1
+
+module_name: TMP102
+
+parameters:
+  - group: Sensors
+    definitions:
+      SENS_EN_TMP102:
+        description:
+            short: Enable TMP102
+            long: |
+                Enable the driver for the TMP102 temperature sensor
+        type: boolean
+        reboot_required: true
+        default: 0
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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 "tmp102.h"
+
+TMP102::TMP102(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config),
+	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
+{
+}
+
+TMP102::~TMP102()
+{
+	ScheduleClear();
+	perf_free(_cycle_perf);
+	perf_free(_comms_errors);
+}
+
+void TMP102::RunImpl()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_cycle_perf);
+
+	float temperature = read_temperature();
+
+	if (std::isnan(temperature)) {
+		perf_count(_comms_errors);
+
+	} else {
+		sensor_temp_s _sensor_temp{};
+		_sensor_temp.timestamp = hrt_absolute_time();
+		_sensor_temp.temperature = temperature;
+		_sensor_temp.device_id = get_device_id();
+		_sensor_temp_pub.publish(_sensor_temp);
+	}
+
+	perf_end(_cycle_perf);
+}
+
+int TMP102::probe()
+{
+	uint16_t conf_reg;
+
+	for (int i = 0; i < 3; i++) {
+		if (read_reg(TMP102_CONFIG_REG, conf_reg) == PX4_OK && (conf_reg | 0x0020) == DEFAULT_CONFIG_REG) { // Mask the AL bit
+			return PX4_OK;
+		}
+
+		px4_sleep(1);
+	}
+
+	return PX4_ERROR;
+}
+
+int TMP102::init()
+{
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		PX4_ERR("TMP102, I2C init failed");
+		return ret;
+	}
+
+	_sensor_temp_pub.advertise();
+	ScheduleOnInterval(250_ms); // DEFAULT SAMPLE RATE IS 4HZ => 250ms INTERVAL
+	return PX4_OK;
+}
+
+float TMP102::read_temperature()
+{
+	uint16_t tmp_data;
+
+	if (read_reg(TMP102_TEMP_REG, tmp_data) != PX4_OK) {
+		return NAN;
+	}
+
+	float temperature = ((int16_t)(tmp_data) >> 4) * 0.0625f;
+	return temperature;
+}
+
+int TMP102::read_reg(uint8_t reg, uint16_t &data)
+{
+	uint8_t tmp_data[2];
+	tmp_data[0] = reg;
+
+	if (transfer(tmp_data, 1, nullptr, 0) != PX4_OK) { // Set the PR to the desired register
+		return PX4_ERROR;
+	}
+
+	int ret = transfer(nullptr, 0, tmp_data, 2); // Read the data from the desired register
+	data = (tmp_data[0] << 8) | tmp_data[1];
+	return ret;
+}
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+#pragma once
+
+#include <stdint.h>
+#include <drivers/device/i2c.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/sensor_temp.h>
+#include <uORB/PublicationMulti.hpp>
+#include <lib/perf/perf_counter.h>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+#define TMP102_TEMP_REG 0x00
+#define TMP102_CONFIG_REG 0x01
+#define TMP102_TLOW_REG 0x02
+#define TMP102_THIGH_REG 0x03
+
+constexpr uint16_t DEFAULT_CONFIG_REG = 0x60A0; // 12-bit resolution, comparator mode, active low, 4Hz
+
+class TMP102 : public device::I2C, public I2CSPIDriver<TMP102>
+{
+public:
+	TMP102(const I2CSPIDriverConfig &config);
+	~TMP102() override;
+
+	int init() override;
+	int probe() override;
+	void RunImpl();
+	static void print_usage();
+
+protected:
+	void print_status();
+
+private:
+	uORB::PublicationMulti<sensor_temp_s> _sensor_temp_pub{ORB_ID(sensor_temp)};
+	perf_counter_t		_cycle_perf;
+	perf_counter_t		_comms_errors;
+
+	float read_temperature();
+	int read_reg(uint8_t address, uint16_t &data);
+};
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tmp102_main.cpp
+ * @author Philipp Engljaehringer
+ *
+ * Driver for the TMP102 Temperature Sensor connected via I2C.
+ */
+
+#include "tmp102.h"
+#include <px4_platform_common/module.h>
+
+void TMP102::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("tmp102", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x48);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+void TMP102::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_comms_errors);
+}
+
+extern "C" int tmp102_main(int argc, char *argv[])
+{
+	using ThisDriver = TMP102;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 400000;
+	cli.i2c_address = 0x48;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_TEMP_DEVTYPE_TMP102);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -43,8 +43,6 @@
 #include <drivers/drv_hrt.h>
 #include <lib/atmosphere/atmosphere.h>

-#define MOTOR_BIT(x) (1<<(x))
-
 using namespace time_literals;

 UavcanEscController::UavcanEscController(uavcan::INode &node) :
@@ -47,13 +47,9 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
-#include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/esc_status.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/uavcan/node_info.hpp>
-#include <parameters/param.h>
+#include "../node_info.hpp"

 class UavcanEscController
 {
@@ -66,10 +66,9 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
 	if (_actuator_armed_sub.update(&actuator_armed)) {
 		uavcan::equipment::safety::ArmingStatus cmd;

-		if (actuator_armed.lockdown || actuator_armed.kill) {
-			cmd.status = cmd.STATUS_DISARMED;
+		bool lockdown_active = actuator_armed.lockdown || actuator_armed.termination || actuator_armed.kill;

-		} else if (actuator_armed.armed) {
+		if (!lockdown_active && (actuator_armed.armed || _is_actuator_test_running)) {
 			cmd.status = cmd.STATUS_FULLY_ARMED;

 		} else {
@@ -57,6 +57,8 @@ public:
 	 */
 	int init();

+	void setActuatorTestRunning(bool running) {_is_actuator_test_running = running;}
+
 private:
 	/*
 	 * Max update rate to avoid exessive bus traffic
@@ -80,4 +82,5 @@ private:

 	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};

+	bool _is_actuator_test_running = false;
 };
@@ -969,6 +969,10 @@ UavcanNode::Run()
 		}
 	}

+#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
+	_arming_status_controller.setActuatorTestRunning(_mixing_interface_esc.isActuatorTestRunning());
+#endif
+
 	perf_end(_cycle_perf);

 	pthread_mutex_unlock(&_node_mutex);
@@ -136,6 +136,8 @@ public:

 	MixingOutput &mixingOutput() { return _mixing_output; }

+	bool isActuatorTestRunning() const { return _mixing_output.isActuatorTestRunning(); }
+
 protected:
 	void Run() override;
 private:
@@ -1,7 +1,9 @@
-from __future__ import print_function
+#!/usr/bin/env python3
+
 from pylab import *
 from pprint import pprint
 import scipy.linalg
+import sys

 # test cases, derived from doc/nasa_rotation_def.pdf

@@ -47,7 +49,7 @@ def quat_prod(q, r):
 def dcm_to_euler(dcm):
    return array([
        arctan(dcm[2,1]/ dcm[2,2]),
-        arctan(-dcm[2,0]/ std::sqrt(1 - dcm[2,0]**2)),
+        arctan(-dcm[2,0]/ (1 - dcm[2,0]**2)**0.5),
        arctan(dcm[1,0]/ dcm[0,0]),
    ])

@@ -75,6 +77,7 @@ psi = 0.3
 print('euler', phi, theta, psi)

 q = euler_to_quat(phi, theta, psi)
+FLT_EPSILON = sys.float_info.epsilon
 assert(abs(norm(q) - 1) < FLT_EPSILON)
 assert(abs(norm(q) - 1) < FLT_EPSILON)
 assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON)
@@ -163,6 +163,7 @@ public:
 	void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us);

 	const actuator_armed_s &armed() const { return _armed; }
+	bool isActuatorTestRunning() const { return _actuator_test.inTestMode(); }

 	void setAllFailsafeValues(uint16_t value);
 	void setAllDisarmedValues(uint16_t value);
@@ -607,13 +607,14 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
 /**
 * Manual control loss exceptions
 *
- * Specify modes where manual control loss is ignored and no failsafe is triggered.
+ * Specify modes in which stick input is ignored and no failsafe action is triggered.
 * External modes requiring stick input will still failsafe.
+ * Auto modes are: Hold, Takeoff, Land, RTL, Descend, Follow Target, Precland, Orbit.
 *
 * @min 0
 * @max 31
 * @bit 0 Mission
- * @bit 1 Hold
+ * @bit 1 Auto modes
 * @bit 2 Offboard
 * @bit 3 External Mode
 * @bit 4 Altitude Cruise
@@ -624,13 +625,16 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
 /**
 * Datalink loss exceptions
 *
- * Specify modes in which datalink loss is ignored and the failsafe action not triggered.
+ * Specify modes in which ground control station connection loss is ignored and no failsafe action is triggered.
+ * See also COM_RCL_EXCEPT.
 *
 * @min 0
- * @max 7
+ * @max 31
 * @bit 0 Mission
- * @bit 1 Hold
+ * @bit 1 Auto modes
 * @bit 2 Offboard
+ * @bit 3 External Mode
+ * @bit 4 Altitude Cruise
 * @group Commander
 */
 PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0);
@@ -182,21 +182,23 @@
                        <tr>
                            <td><label for="intended_nav_state">Intended Mode:&nbsp;</label></td>
                            <td><select id="intended_nav_state">
-                                <option value="0">MANUAL</option>
-                                <option value="10">ACRO</option>
-                                <option value="15">STAB</option>
-                                <option value="1">ALTCTL</option>
-                                <option value="2" selected>POSCTL</option>
-                                <option value="3">AUTO_MISSION</option>
-                                <option value="4">AUTO_LOITER</option>
-                                <option value="5">AUTO_RTL</option>
-                                <option value="17">AUTO_TAKEOFF</option>
-                                <option value="18">AUTO_LAND</option>
-                                <option value="19">AUTO_FOLLOW_TARGET</option>
-                                <option value="20">AUTO_PRECLAND</option>
-                                <option value="22">AUTO_VTOL_TAKEOFF</option>
-                                <option value="14">OFFBOARD</option>
-                                <option value="21">ORBIT</option>
+                                <option value="0">Manual</option>
+                                <option value="10">Acro</option>
+                                <option value="15">Stabilized</option>
+                                <option value="1">Altitude</option>
+                                <option value="8">Altitude Cruise</option>
+                                <option value="2" selected>Position</option>
+                                <option value="3">Mission</option>
+                                <option value="4">Hold</option>
+                                <option value="5">Return</option>
+                                <option value="17">Takeoff</option>
+                                <option value="18">Land</option>
+                                <option value="19">Follow</option>
+                                <option value="20">Precision Land</option>
+                                <option value="22">VTOL Takeoff</option>
+                                <option value="14">Offboard</option>
+                                <option value="21">Orbit</option>
+                                <option value="23">External 1</option>
                            </select>
                            </td>
                            <td></td>
@@ -442,18 +442,53 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par
 	return options;
 }

+bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter)
+{
+	switch (user_intended_mode) {
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+		return exception_mask_parameter & (int)LinkLossExceptionBits::Mission;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
+	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
+		return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes;
+
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
+		return exception_mask_parameter & (int)LinkLossExceptionBits::Offboard;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
+		return exception_mask_parameter & (int)LinkLossExceptionBits::ExternalMode;
+
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
+		return exception_mask_parameter & (int)LinkLossExceptionBits::AltitudeCruise;
+
+	default:
+		return false;
+	}
+}
+
 void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 				 const failsafe_flags_s &status_flags)
 {
 	updateArmingState(time_us, state.armed, status_flags);

-	const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
-				       || state.vtol_in_transition_mode;
-
 	// Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
 	// altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
-	const bool ignore_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode ==
-			vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
+	const bool in_forward_flight = (state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || state.vtol_in_transition_mode;
+	const bool ignore_any_link_loss_vtol_takeoff_fixedwing = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 			&& in_forward_flight && !state.mission_finished;

 	// Manual control (RC or joystick) loss
@@ -462,59 +497,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		_manual_control_lost_at_arming = false;
 	}

-	const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
-					     && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission);
-	const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
-					    && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
-	const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
-					      && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard);
-	const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
-					      state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
-					     && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
-	const bool rc_loss_ignored_altitude_cruise = (state.user_intended_mode ==
-			vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE
-			&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AltitudeCruise));
-
-	const bool rc_loss_ignored_external_mode =
-		(state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL2 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL3 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL4 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL5 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL6 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL7 ||
-		 state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8)
-		&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode);
-
-	const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
-				     rc_loss_ignored_takeoff || rc_loss_ignored_external_mode || ignore_any_link_loss_vtol_takeoff_fixedwing
-				     || _manual_control_lost_at_arming || rc_loss_ignored_altitude_cruise;
+	const bool rc_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get())
+				     || ignore_any_link_loss_vtol_takeoff_fixedwing || _manual_control_lost_at_arming;

 	if (_param_com_rc_in_mode.get() != int32_t(RcInMode::DisableManualControl) && !rc_loss_ignored) {
 		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
 			       fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
 	}

-	// GCS connection loss
+	// Ground control station connection loss
 	const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
 					   || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;

-	const bool dll_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
-					      && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Mission);
-	const bool dll_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
-					     && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
-	const bool dll_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
-					       && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Offboard);
-	const bool dll_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
-					       state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
-					      && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
-
-	const bool dll_loss_ignored = dll_loss_ignored_mission || dll_loss_ignored_loiter || dll_loss_ignored_offboard ||
-				      dll_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;
+	const bool dll_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_dll_except.get())
+				      || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;

 	if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) {
-		CHECK_FAILSAFE(status_flags, gcs_connection_lost,
-			       fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
+		CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
 	}

 	// VTOL transition failure (quadchute)
@@ -531,7 +530,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,

 		// If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished,
 		// trigger RTL to avoid losing the vehicle
-		if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) || rc_loss_ignored_mission)
+		if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl)
+		     || isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get()))
 		    && _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled)
 		    && state.mission_finished) {
 			_last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost,
@@ -53,20 +53,14 @@ protected:
 private:
 	void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags);

-	enum class ManualControlLossExceptionBits : int32_t {
+	enum class LinkLossExceptionBits : int32_t {
 		Mission = (1 << 0),
-		Hold = (1 << 1),
+		AutoModes = (1 << 1),
 		Offboard = (1 << 2),
 		ExternalMode = (1 << 3),
 		AltitudeCruise = (1 << 4)
 	};

-	enum class DatalinkLossExceptionBits : int32_t {
-		Mission = (1 << 0),
-		Hold = (1 << 1),
-		Offboard = (1 << 2)
-	};
-
 	// COM_LOW_BAT_ACT parameter values
 	enum class LowBatteryAction : int32_t {
 		Warning = 0,        // Warning
@@ -175,6 +169,8 @@ private:
 	static ActionOptions fromPosLowActParam(int param_value);
 	static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);

+	static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
+
 	const int _caller_id_mode_fallback{genCallerId()};
 	bool _last_state_mode_fallback{false};
 	const int _caller_id_mission_control_lost{genCallerId()};
@@ -108,13 +108,6 @@ bool FlightTaskAuto::update()
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();

-	// The only time a thrust set-point is sent out is during
-	// idle. Hence, reset thrust set-point to NAN in case the
-	// vehicle exits idle.
-	if (_type_previous == WaypointType::idle) {
-		_acceleration_setpoint.setNaN();
-	}
-
 	// during mission and reposition, raise the landing gears but only
 	// if altitude is high enough
 	if (_highEnoughForLandingGear()) {
@@ -122,13 +115,6 @@ bool FlightTaskAuto::update()
 	}

 	switch (_type) {
-	case WaypointType::idle:
-		// Send zero thrust setpoint
-		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
-		_velocity_setpoint.setNaN();
-		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-		break;
-
 	case WaypointType::land:
 		_prepareLandSetpoints();
 		break;
@@ -64,8 +64,7 @@ enum class WaypointType : int {
 	velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
 	loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
 	takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
-	land = position_setpoint_s::SETPOINT_TYPE_LAND,
-	idle = position_setpoint_s::SETPOINT_TYPE_IDLE
+	land = position_setpoint_s::SETPOINT_TYPE_LAND
 };

 enum class State {
@@ -129,7 +128,7 @@ protected:
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
 	bool _next_was_valid{false};
 	float _mc_cruise_speed{NAN}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
-	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
+	WaypointType _type{WaypointType::position}; /**< Type of current target triplet. */

 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
@@ -148,7 +147,7 @@ protected:
 	StickYaw _stick_yaw{this};
 	matrix::Vector3f _land_position;
 	float _land_heading;
-	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
+	WaypointType _type_previous{WaypointType::position}; /**< Previous type of current target triplet. */
 	bool _is_emergency_braking_active{false};
 	bool _want_takeoff{false};

@@ -166,6 +166,8 @@ void FwLateralLongitudinalControl::Run()
 			_landed = landed.landed;
 		}

+		uint8_t current_flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
+
 		_vehicle_status_sub.update();
 		_control_mode_sub.update();

@@ -209,18 +211,18 @@ void FwLateralLongitudinalControl::Run()
 			// If the both altitude and height rate are set, set altitude setpoint to NAN
 			const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude;

-			tecs_update_pitch_throttle(control_interval, altitude_sp,
-						   airspeed_sp_eas,
-						   _long_configuration.pitch_min,
-						   _long_configuration.pitch_max,
-						   _long_configuration.throttle_min,
-						   _long_configuration.throttle_max,
-						   _long_configuration.sink_rate_target,
-						   _long_configuration.climb_rate_target,
-						   _long_configuration.disable_underspeed_protection,
-						   _long_control_sp.height_rate,
-						   now
-						  );
+			current_flight_phase = tecs_update_pitch_throttle(control_interval, altitude_sp,
+					       airspeed_sp_eas,
+					       _long_configuration.pitch_min,
+					       _long_configuration.pitch_max,
+					       _long_configuration.throttle_min,
+					       _long_configuration.throttle_max,
+					       _long_configuration.sink_rate_target,
+					       _long_configuration.climb_rate_target,
+					       _long_configuration.disable_underspeed_protection,
+					       _long_control_sp.height_rate,
+					       now
+									 );

 			pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
 			throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct :
@@ -317,6 +319,16 @@ void FwLateralLongitudinalControl::Run()

 		}

+		// Publish flight phase with low rate, but immediately if updated
+		const bool flight_phase_updated = current_flight_phase != _flight_phase_estimation_pub.get().flight_phase;
+		const hrt_abstime time_since_last_flightphase_pub = now - _flight_phase_estimation_pub.get().timestamp;
+
+		if (flight_phase_updated || time_since_last_flightphase_pub >= 1_s) {
+			_flight_phase_estimation_pub.get().timestamp = now;
+			_flight_phase_estimation_pub.get().flight_phase = current_flight_phase;
+			_flight_phase_estimation_pub.update();
+		}
+
 		_z_reset_counter = _local_pos.z_reset_counter;
 	}

@@ -356,7 +368,7 @@ void FwLateralLongitudinalControl::updateControllerConfiguration(hrt_abstime tim
 	}
 }

-void
+uint8_t
 FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad, float throttle_min,
 		float throttle_max, const float desired_max_sinkrate,
@@ -370,8 +382,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 		|| _vehicle_status_sub.get().in_transition_mode)) {
 		tecs_is_running = false;
-		return;
-
+		return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
 	}

 	const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
@@ -409,25 +420,23 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 		// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
 		if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
 		    fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
-			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
+			return flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;

 		} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
 			   (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
-			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
+			return flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;

 		} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
 			   (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
-			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
+			return flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;

 		} else {
-			// We can't infer the flight phase , do nothing, estimation is reset at each step
-			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
-
+			// We can't infer the flight phase
+			return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
 		}
-
-		_flight_phase_estimation_pub.get().timestamp = now;
-		_flight_phase_estimation_pub.update();
 	}
+
+	return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
 }

 void
@@ -213,11 +213,12 @@ private:

 	void parameters_update();
 	void update_control_state(hrt_abstime now);
-	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
-					float pitch_min_rad, float pitch_max_rad, float throttle_min,
-					float throttle_max, const float desired_max_sinkrate,
-					const float desired_max_climbrate,
-					bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);
+
+	uint8_t tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+					   float pitch_min_rad, float pitch_max_rad, float throttle_min,
+					   float throttle_max, const float desired_max_sinkrate,
+					   const float desired_max_climbrate,
+					   bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);

 	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
 				 float throttle_trim, hrt_abstime now);
@@ -387,8 +387,7 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 			return;
 		}

-	} else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-		   && _position_setpoint_current_valid) {
+	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled && _position_setpoint_current_valid) {

 		// Enter this mode only if the current waypoint has valid 3D position setpoints.

@@ -431,17 +430,6 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 			_control_mode_current = FW_POSCTRL_MODE_AUTO;
 		}

-	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled
-		   && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
-
-		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
-		if (doing_backtransition) {
-			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
-
-		} else {
-			_control_mode_current = FW_POSCTRL_MODE_AUTO;
-		}
-
 	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled
 		   && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {

@@ -577,11 +565,6 @@ FixedWingModeManager::control_auto(const float control_interval, const Vector2d
 	}

 	switch (position_sp_type) {
-	case position_setpoint_s::SETPOINT_TYPE_IDLE: {
-			control_idle();
-			break;
-		}
-
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
 		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
@@ -621,24 +604,6 @@ FixedWingModeManager::control_auto(const float control_interval, const Vector2d
 	}
 }

-void FixedWingModeManager::control_idle()
-{
-	const hrt_abstime  now = hrt_absolute_time();
-	fixed_wing_lateral_setpoint_s lateral_ctrl_sp {empty_lateral_control_setpoint};
-	lateral_ctrl_sp.timestamp = now;
-	lateral_ctrl_sp.lateral_acceleration = 0.0f;
-	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
-
-	fixed_wing_longitudinal_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint};
-	long_contrl_sp.timestamp = now;
-	long_contrl_sp.pitch_direct = 0.f;
-	long_contrl_sp.throttle_direct = 0.0f;
-	_longitudinal_ctrl_sp_pub.publish(long_contrl_sp);
-
-	_ctrl_configuration_handler.setThrottleMax(0.0f);
-	_ctrl_configuration_handler.setThrottleMin(0.0f);
-}
-
 void
 FixedWingModeManager::control_auto_fixed_bank_alt_hold()
 {
@@ -808,7 +773,10 @@ FixedWingModeManager::control_auto_position(const float control_interval, const
 	float throttle_min = NAN;
 	float throttle_max = NAN;

-	if (pos_sp_curr.gliding_enabled) {
+	if (_landed) {
+		throttle_max = _param_fw_thr_idle.get();
+
+	} else if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		throttle_min = 0.0;
 		throttle_max = 0.0;
@@ -868,7 +836,10 @@ FixedWingModeManager::control_auto_velocity(const float control_interval, const

 	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);

-	if (pos_sp_curr.gliding_enabled) {
+	if (_landed) {
+		_ctrl_configuration_handler.setThrottleMax(_param_fw_thr_idle.get());
+
+	} else if (pos_sp_curr.gliding_enabled) {
 		_ctrl_configuration_handler.setThrottleMin(0.0f);
 		_ctrl_configuration_handler.setThrottleMax(0.0f);
 		_ctrl_configuration_handler.setSpeedWeight(2.0f);
@@ -962,7 +933,10 @@ FixedWingModeManager::control_auto_loiter(const float control_interval, const Ve

 	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);

-	if (pos_sp_curr.gliding_enabled) {
+	if (_landed) {
+		_ctrl_configuration_handler.setThrottleMax(_param_fw_thr_idle.get());
+
+	} else if (pos_sp_curr.gliding_enabled) {
 		_ctrl_configuration_handler.setThrottleMin(0.0f);
 		_ctrl_configuration_handler.setThrottleMax(0.0f);
 		_ctrl_configuration_handler.setSpeedWeight(2.0f);
@@ -1010,7 +984,10 @@ FixedWingModeManager::controlAutoFigureEight(const float control_interval, const

 	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);

-	if (pos_sp_curr.gliding_enabled) {
+	if (_landed) {
+		_ctrl_configuration_handler.setThrottleMax(_param_fw_thr_idle.get());
+
+	} else if (pos_sp_curr.gliding_enabled) {
 		_ctrl_configuration_handler.setThrottleMin(0.0f);
 		_ctrl_configuration_handler.setThrottleMax(0.0f);
 		_ctrl_configuration_handler.setSpeedWeight(2.0f);
@@ -125,7 +125,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("sensor_gyro_fft", 50);
 	add_topic("sensor_selection");
 	add_topic("sensors_status_imu", 200);
-	add_optional_topic("sensor_temp", 100);
 	add_optional_topic("spoilers_setpoint", 1000);
 	add_topic("system_power", 500);
 	add_optional_topic("takeoff_status", 1000);
@@ -167,6 +166,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("control_allocator_status", 200, 2);
 	add_optional_topic_multi("rate_ctrl_status", 200, 2);
 	add_optional_topic_multi("sensor_hygrometer", 500, 4);
+	add_optional_topic_multi("sensor_temp", 100, 4);
 	add_optional_topic_multi("rpm", 200);
 	add_topic_multi("timesync_status", 1000, 3);
 	add_optional_topic_multi("telemetry_status", 1000, 4);
@@ -243,8 +243,7 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item,
 	}

 	// check if we find unsupported items and reject mission if so
-	if (mission_item.nav_cmd != NAV_CMD_IDLE &&
-	    mission_item.nav_cmd != NAV_CMD_WAYPOINT &&
+	if (mission_item.nav_cmd != NAV_CMD_WAYPOINT &&
 	    mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
 	    mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
 	    mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
@@ -344,8 +343,7 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item)
 	}

 	if (!_found_item_with_position) {
-		_found_item_with_position = (mission_item.nav_cmd != NAV_CMD_IDLE &&
-					     mission_item.nav_cmd != NAV_CMD_DELAY &&
+		_found_item_with_position = (mission_item.nav_cmd != NAV_CMD_DELAY &&
 					     mission_item.nav_cmd != NAV_CMD_DO_JUMP &&
 					     mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
 					     mission_item.nav_cmd != NAV_CMD_DO_SET_HOME &&
@@ -94,7 +94,6 @@ Land::on_active()
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 		_navigator->mode_completed(getNavigatorStateId());
-		set_idle_item(&_mission_item);

 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -76,45 +76,27 @@ Loiter::on_active()
 void
 Loiter::set_loiter_position()
 {
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED &&
-	    _navigator->get_land_detected()->landed) {
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

-		// Not setting loiter position if disarmed and landed, instead mark the current
-		// setpoint as invalid and idle (both, just to be sure).
+	// Check if we already loiter on a circle and are on the loiter pattern.
+	bool on_loiter{false};

-		_navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
-		_navigator->set_position_setpoint_triplet_updated();
-		return;
+	if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER
+	    && pos_sp_triplet->current.loiter_pattern == position_setpoint_s::LOITER_TYPE_ORBIT) {
+		const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
+					_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+		on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius);

 	}

-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	if (on_loiter) {
+		setLoiterItemFromCurrentPositionSetpoint(&_mission_item);

-	if (_navigator->get_land_detected()->landed) {
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		setLoiterItemFromCurrentPositionWithBraking(&_mission_item);

 	} else {
-		// Check if we already loiter on a circle and are on the loiter pattern.
-		bool on_loiter{false};
-
-		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER
-		    && pos_sp_triplet->current.loiter_pattern == position_setpoint_s::LOITER_TYPE_ORBIT) {
-			const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
-						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-			on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius);
-
-		}
-
-		if (on_loiter) {
-			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
-
-		} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			setLoiterItemFromCurrentPositionWithBraking(&_mission_item);
-
-		} else {
-			setLoiterItemFromCurrentPosition(&_mission_item);
-		}
-
+		setLoiterItemFromCurrentPosition(&_mission_item);
 	}

 	// convert mission item to current setpoint
@@ -552,10 +552,8 @@ void MissionBase::setEndOfMissionItems()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

-	if (_land_detected_sub.get().landed) {
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-	} else {
+	// Set loiter item only if not landed
+	if (!_land_detected_sub.get().landed) {
 		if (pos_sp_triplet->current.valid &&
 		    (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER ||
 		     pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)) {
@@ -106,7 +106,6 @@ MissionBlock::is_mission_item_reached_or_completed()
 	case NAV_CMD_VTOL_LAND:
 		return _navigator->get_land_detected()->landed;

-	case NAV_CMD_IDLE: /* fall through */
 	case NAV_CMD_LOITER_UNLIMITED:
 		return false;

@@ -650,10 +649,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->gliding_enabled = (_navigator->get_cruising_throttle() < FLT_EPSILON);

 	switch (item.nav_cmd) {
-	case NAV_CMD_IDLE:
-		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
-		break;
-
 	case NAV_CMD_TAKEOFF:
 	case NAV_CMD_VTOL_TAKEOFF:

@@ -816,22 +811,6 @@ MissionBlock::set_land_item(struct mission_item_s *item)
 	item->origin = ORIGIN_ONBOARD;
 }

-void
-MissionBlock::set_idle_item(struct mission_item_s *item)
-{
-	item->nav_cmd = NAV_CMD_IDLE;
-	item->lat = _navigator->get_home_position()->lat;
-	item->lon = _navigator->get_home_position()->lon;
-	item->altitude_is_relative = false;
-	item->altitude = _navigator->get_home_position()->alt;
-	item->yaw = NAN;
-	item->loiter_radius = _navigator->get_default_loiter_rad();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->time_inside = 0.0f;
-	item->autocontinue = true;
-	item->origin = ORIGIN_ONBOARD;
-}
-
 void
 MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode)
 {
@@ -1002,7 +981,7 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i

 	if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND
 	    && _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION
-	    && _mission_item.nav_cmd != NAV_CMD_IDLE
+	    && !_navigator->get_land_detected()->landed
 	    && _navigator->get_local_position()->dist_bottom_valid
 	    && _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param()
 	    && _navigator->get_local_position()->vz > FLT_EPSILON
@@ -178,11 +178,6 @@ protected:
 	 */
 	void set_land_item(struct mission_item_s *item);

-	/**
-	 * Set idle mission item
-	 */
-	void set_idle_item(struct mission_item_s *item);
-
 	/**
 	 * Set vtol transition item
 	 */
@@ -48,7 +48,6 @@

 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
-	NAV_CMD_IDLE = 0,
 	NAV_CMD_WAYPOINT = 16,
 	NAV_CMD_LOITER_UNLIMITED = 17,
 	NAV_CMD_LOITER_TIME_LIMIT = 19,
@@ -1200,7 +1200,7 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.cruising_speed = get_cruising_speed();
 	sp.cruising_throttle = get_cruising_throttle();
 	sp.valid = false;
-	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
+	sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 	sp.loiter_direction_counter_clockwise = false;
 }

@@ -379,7 +379,6 @@ void RtlDirect::set_rtl_item()
 		}

 	case RTLState::IDLE: {
-			set_idle_item(&_mission_item);
 			_navigator->mode_completed(getNavigatorStateId());
 			break;
 		}
@@ -157,11 +157,7 @@ Takeoff::on_active()

 			position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

-			// set loiter item so position controllers stop doing takeoff logic
-			if (_navigator->get_land_detected()->landed) {
-				_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-			} else {
+			if (!_navigator->get_land_detected()->landed) {
 				if (pos_sp_triplet->current.valid
 				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 					setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
@@ -76,7 +76,7 @@ void AckermannAutoMode::autoControl()
 		rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
 		rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
 		rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
-		rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
+		rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type, _curr_wp_valid,
 							_waypoint_transition_angle, _max_yaw_rate);
 		rover_position_setpoint.cruising_speed = _cruising_speed;
 		rover_position_setpoint.yaw = NAN;
@@ -90,6 +90,8 @@ void AckermannAutoMode::updateWaypointsAndAcceptanceRadius()
 	position_setpoint_triplet_s position_setpoint_triplet{};
 	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
 	_curr_wp_type = position_setpoint_triplet.current.type;
+	_curr_wp_valid = position_setpoint_triplet.current.valid;
+

 	RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
 			_curr_pos_ned, _global_ned_proj_ref);
@@ -136,11 +138,10 @@ float AckermannAutoMode::updateAcceptanceRadius(const float waypoint_transition_
 }

 float AckermannAutoMode::arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad,
-				      const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
+				      const int curr_wp_type, const bool curr_wp_valid, const float waypoint_transition_angle, const float max_yaw_rate)
 {
 	if (!PX4_ISFINITE(waypoint_transition_angle)
-	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || !curr_wp_valid) {
 		return 0.f; // Stop at the waypoint

 	} else if (_param_ro_speed_red.get() > FLT_EPSILON) {
@@ -98,12 +98,13 @@ private:
 	 * @param min_speed Minimum speed setpoint [m/s].
 	 * @param acc_rad Acceptance radius of the current waypoint [m].
 	 * @param curr_wp_type Type of the current waypoint.
+	 * @param curr_wp_valid Validity flag of the current waypoint.
 	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	 * @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
 	 * @return Speed setpoint [m/s].
 	 */
-	float arrivalSpeed(float cruising_speed, float min_speed, float acc_rad, int curr_wp_type,
-			   float waypoint_transition_angle, float max_yaw_rate);
+	float arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad, const int curr_wp_type, const bool curr_wp_valid,
+			   const float waypoint_transition_angle, const float max_yaw_rate);

 	// uORB subscriptions
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -124,7 +125,8 @@ private:
 	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	float _max_yaw_rate{NAN};
 	float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
-	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
+	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_POSITION};
+	int _curr_wp_valid{false};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
@@ -51,7 +51,8 @@ void DifferentialAutoMode::autoControl()
 	if (_position_setpoint_triplet_sub.updated()) {
 		position_setpoint_triplet_s position_setpoint_triplet{};
 		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
-		int curr_wp_type = position_setpoint_triplet.current.type;
+		const int curr_wp_type = position_setpoint_triplet.current.type;
+		const bool curr_wp_valid = position_setpoint_triplet.current.valid;

 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
@@ -85,7 +86,7 @@ void DifferentialAutoMode::autoControl()
 		rover_position_setpoint.start_ned[0] = prev_wp_ned(0);
 		rover_position_setpoint.start_ned[1] = prev_wp_ned(1);
 		rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle,
-							_param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type);
+							_param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type, curr_wp_valid);
 		rover_position_setpoint.cruising_speed = cruising_speed;
 		rover_position_setpoint.yaw = NAN;
 		_rover_position_setpoint_pub.publish(rover_position_setpoint);
@@ -93,11 +94,11 @@ void DifferentialAutoMode::autoControl()
 }

 float DifferentialAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
-		const float max_speed, const float trans_drv_trn, const float speed_red, int curr_wp_type)
+		const float max_speed, const float trans_drv_trn, const float speed_red, const int curr_wp_type, const bool curr_wp_valid)
 {
 	// Upcoming stop
 	if (!PX4_ISFINITE(waypoint_transition_angle) || waypoint_transition_angle < M_PI_F - trans_drv_trn
-	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || !curr_wp_valid) {
 		return 0.f;
 	}

@@ -81,10 +81,11 @@ private:
 	 * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad].
 	 * @param speed_red Tuning parameter for the speed reduction during waypoint transition.
 	 * @param curr_wp_type Type of the current waypoint.
+	 * @param curr_wp_valid Validity flag of the current waypoint.
 	 * @return Speed setpoint [m/s].
 	 */
 	float arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed,
-			   const float trans_drv_trn, const float speed_red, int curr_wp_type);
+			   const float trans_drv_trn, const float speed_red, const int curr_wp_type, const bool curr_wp_valid);

 	// uORB subscriptions
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -51,7 +51,8 @@ void MecanumAutoMode::autoControl()
 	if (_position_setpoint_triplet_sub.updated()) {
 		position_setpoint_triplet_s position_setpoint_triplet{};
 		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
-		int curr_wp_type = position_setpoint_triplet.current.type;
+		const int curr_wp_type = position_setpoint_triplet.current.type;
+		const bool curr_wp_valid = position_setpoint_triplet.current.valid;

 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
@@ -85,7 +86,7 @@ void MecanumAutoMode::autoControl()
 		rover_position_setpoint.start_ned[0] = prev_wp_ned(0);
 		rover_position_setpoint.start_ned[1] = prev_wp_ned(1);
 		rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle,
-							_param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type);
+							_param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type, curr_wp_valid);
 		rover_position_setpoint.cruising_speed = cruising_speed;
 		rover_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ?
 					      position_setpoint_triplet.current.yaw : NAN;
@@ -94,11 +95,10 @@ void MecanumAutoMode::autoControl()
 }

 float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
-				    const float max_speed, const float speed_red, int curr_wp_type)
+				    const float max_speed, const float speed_red, const int curr_wp_type, const bool curr_wp_valid)
 {
 	// Upcoming stop
-	if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || !curr_wp_valid) {
 		return 0.f;
 	}

@@ -80,10 +80,11 @@ private:
 	 * @param max_speed Maximum speed setpoint [m/s]
 	 * @param speed_red Tuning parameter for the speed reduction during waypoint transition.
 	 * @param curr_wp_type Type of the current waypoint.
+	 * @param curr_wp_valid validity flag of the current waypoint.
 	 * @return Speed setpoint [m/s].
 	 */
 	float arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed,
-			   const float speed_red, int curr_wp_type);
+			   const float speed_red, const int curr_wp_type, const bool curr_wp_valid);

 	// uORB subscriptions
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
Author	SHA1	Message	Date
Balduin	560bffa823	FixedWingModeManager: remove IDLE handling that moved in rebase	2026-01-14 14:46:12 +01:00
Balduin	df6e432f71	FixedWingModeManager: give idle throttle if landed - except in takeoff where that is nonsensical - the other control_auto* and the two control_manual* functions already have such logic	2026-01-14 13:59:45 +01:00
Balduin	80c8e42de3	mission_block: replace idle with landed check which is the alternative solution mentioned in the original PR: https://github.com/PX4/PX4-Autopilot/pull/23674	2026-01-14 13:59:45 +01:00
Balduin	db8e31eaec	differential: replace previous waypoint type == idle check with validity check	2026-01-14 13:59:45 +01:00
Balduin	9b0e1b8378	ackermann: replace previous waypoint type == idle check with validity check	2026-01-14 13:59:45 +01:00
Balduin	1433931fe0	mecanum: replace previous waypoint type == idle check with validity check	2026-01-14 13:59:45 +01:00
Balduin	503f8d8fc7	remove SETPOINT_TYPE_IDLE everywhere along with NAV_CMD_IDLE, set_idle_item, and WaypointType::idle which are basically the same	2026-01-14 13:59:45 +01:00
jmackay2	e371c4edd9	Fix the test data matrix script (#24745 ) Co-authored-by: jmackay2 <jmackay2@gmail.com>	2026-01-13 13:47:24 -09:00
Matthias Grob	473ef5fd06	uavcan: esc: fix actuator test on uavcan ESCs that consume `ArmingStatus` (#26255 ) * uavcan esc: remove unused includes * uavcan arming_status: disarm when terminated To stay consistent with kill. * uavcan: publish armed during actuator tests to make it possible spinning motors	2026-01-13 10:57:54 -09:00
Phil-Engljaehringer	46d9b14ba0	Feat: Add driver for TMP102 temperature sensor for Skynode-N (#26241 ) * feat: added driver for tmp102 temperature sensor * style: removed new line * style: adjusted date in header * style: removed duplicated logging * fix: moved start-up command from rc.board_sensors to rc.sensors * style: used consexpr for expected config reg value * feat: added retry logic to probe function * style: added _ as prefix to global variable * style: used make format * fix: corrected temperature calculation * fix: mask AL-bit in probe function * style: removed header files from CMakeLists * style: used correct english in comments * refactor: return error right after failure * style: moved init call to correct place * fix: corrected temperature calculation (again) * refactor: removed _curr_pr variable => always have to set PR to desired register on read * fix: add multi logged topic	2026-01-12 18:42:51 +01:00
Balduin	66e21497a6	FwLateralLongitudinalControl: publish flight phase also if unknown, and with limited rate (#26251 ) * FwLateralLongitudinalControl: publish uknown flight phase if TECS not running * FwLateralLongitudinalControl: publish flight phase with lower rate For this we store the new flight phase in a local variable, which is returned by tecs_update_pitch_throttle (but initialised outside to unknown in case TECS does not run).	2026-01-12 15:30:26 +01:00
Matthias Grob	de49edc428	failsafe web simulation: end user friendly mode names + add Altitude cruise and External 1	2026-01-12 11:30:07 +01:00
Matthias Grob	b5846fd8c2	Commander: unify RC loss and data link exception options	2026-01-12 11:30:07 +01:00
Silvan	ec6dd286fc	Commander: COM_RCL_EXCEPT consider all auto modes triggered by action in bit 1 Signed-off-by: Silvan <silvan@auterion.com>	2026-01-12 11:30:07 +01:00