Made a separate precision landing library. Now, the Precision Landing mode can use it the same as the RTL and the mission mode.

arm

- Previously, due to the way MAVSDK's `health_all_ok` was implemented,
vehicle often didn't have a valid global position estimate (although
function returned true), and it wouldn't arm, and the SITL would fail
- Also sometimes as vehicle didn't have manual control, it entered weird
states where it wasn't able to arm as well
- This adds a check to make sure vehicle is able to arm, directly from
the Health struct

.github/ISSUE_TEMPLATE/2_Feature_request.md

@@ -5,7 +5,7 @@ labels: feature-request
 
 ---
 
-For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (you can find an invite link on this project README).
+For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Discord (you can find an invite link on this project README).
 
 ## Describe problem solved by the proposed feature
 A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...

.github/PULL_REQUEST_TEMPLATE.md

@@ -1,4 +1,4 @@
-Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
+Please use [PX4 Discuss](http://discuss.px4.io/) or [Discord](https://discord.gg/dronecode) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
 
 ## Describe problem solved by this pull request
 A clear and concise description of the problem this proposed change will solve. Or, what it will improve.

Jenkinsfile

@@ -94,21 +94,22 @@ pipeline {
 
         stage('failsafe docs') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
+            docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' }
           }
           steps {
             sh '''#!/bin/bash -l
-			echo $0;
+            echo $0;
             git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk;
             cd _emscripten_sdk;
             ./emsdk install latest;
             ./emsdk activate latest;
+            cd ..;
             . ./_emscripten_sdk/emsdk_env.sh;
-			make failsafe_web;
-			cd build/px4_sitl_default_failsafe_web;
-			mkdir -p failsafe_sim;
-			cp index.* parameters.json failsafe_sim;
-			'''
+            make failsafe_web;
+            cd build/px4_sitl_default_failsafe_web;
+            mkdir -p failsafe_sim;
+            cp index.* parameters.json failsafe_sim;
+            '''
             dir('build/px4_sitl_default_failsafe_web') {
               archiveArtifacts(artifacts: 'failsafe_sim/*')
               stash includes: 'failsafe_sim/*', name: 'failsafe_sim'

ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert

@@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter

@@ -70,7 +70,7 @@ param set-default MPC_XY_VEL_D_ACC 0.1
 param set-default NAV_ACC_RAD 5
 
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.5
+param set-default VT_FW_DIFTHR_S_Y 0.5
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -22,7 +22,7 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -191,7 +191,10 @@ then
 	ist8308 -X -q start
 	ist8310 -X -q start
 	lis2mdl -X -q start
-	lis3mdl -X -q start
+	if ! lis3mdl -X -q start
+	then
+		lis3mdl -X -q -a 0x1c start
+	fi
 	qmc5883l -X -q start
 	rm3100 -X -q start
 

Tools/px_uploader.py

@@ -582,7 +582,35 @@ class uploader(object):
         self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
 
     # upload the firmware
-    def upload(self, fw, force=False, boot_delay=None):
+    def upload(self, fw_list, force=False, boot_delay=None, boot_check=False):
+        # select correct binary
+        found_suitable_firmware = False
+        for file in fw_list:
+            fw = firmware(file)
+            if self.board_type == fw.property('board_id'):
+                if len(fw_list) > 1: print("using firmware binary {}".format(file))
+                found_suitable_firmware = True
+                break
+
+        if not found_suitable_firmware:
+            msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % (
+                self.board_type, fw.property('board_id'))
+            print("WARNING: %s" % msg)
+            if force:
+                if len(fw_list) > 1:
+                    raise FirmwareNotSuitableException("force flashing failed, more than one file provided, none suitable")
+                print("FORCED WRITE, FLASHING ANYWAY!")
+            else:
+                raise FirmwareNotSuitableException(msg)
+
+        percent = fw.property('image_size') / fw.property('image_maxsize')
+        binary_size = float(fw.property('image_size'))
+        binary_max_size = float(fw.property('image_maxsize'))
+        percent = (binary_size / binary_max_size) * 100
+
+        print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
+        print()
+
         # Make sure we are doing the right thing
         start = _time()
         if self.board_type != fw.property('board_id'):
@@ -764,7 +792,7 @@ def main():
     parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
     parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
     parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
-    parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
+    parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)")
     args = parser.parse_args()
 
     if args.use_protocol_splitter_format:
@@ -776,17 +804,7 @@ def main():
         print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
         print("==========================================================================================================")
 
-    # Load the firmware file
-    fw = firmware(args.firmware)
-
-    percent = fw.property('image_size') / fw.property('image_maxsize')
-    binary_size = float(fw.property('image_size'))
-    binary_max_size = float(fw.property('image_maxsize'))
-    percent = (binary_size / binary_max_size) * 100
-
-    print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%), waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
-    print()
-
+    print("Waiting for bootloader...")
     # tell any GCS that might be connected to the autopilot to give up
     # control of the serial port
 
@@ -889,7 +907,7 @@ def main():
 
                 try:
                     # ok, we have a bootloader, try flashing it
-                    up.upload(fw, force=args.force, boot_delay=args.boot_delay)
+                    up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay)
 
                     # if we made this far without raising exceptions, the upload was successful
                     successful = True

boards/modalai/voxl2/default.px4board

@@ -2,3 +2,4 @@ CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
+CONFIG_SYSTEMCMDS_PERF=y

boards/nxp/ucans32k146/src/board_config.h

@@ -99,14 +99,14 @@ __BEGIN_DECLS
 
 /* Timer I/O PWM and capture
  *
- * ?? PWM outputs are configured.
+ * 2 PWM outputs are configured.
  * ?? Timer inputs are configured.
  *
  * Pins:
  *      Defined in board.h
  */
 
-#define DIRECT_PWM_OUTPUT_CHANNELS  1
+#define DIRECT_PWM_OUTPUT_CHANNELS  2
 
 
 #define BOARD_HAS_LED_PWM              1

boards/nxp/ucans32k146/src/timer_config.cpp

@@ -105,11 +105,13 @@
 #define rPWMLOAD(t)    REG(t,S32K1XX_FTM_PWMLOAD_OFFSET)
 
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::FTM1),
 	initIOTimer(Timer::FTM2),
 };
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::FTM1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin1}),
 };
 
 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -154,17 +156,26 @@ void ucans32k_timer_initialize(void)
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM0_SC) = regval;
 
+	regval = _REG(S32K1XX_FTM1_SC);
+	regval &= ~(FTM_SC_CLKS_MASK);
+	regval |= FTM_SC_CLKS_FTM;
+	_REG(S32K1XX_FTM1_SC) = regval;
+
 	regval = _REG(S32K1XX_FTM2_SC);
 	regval &= ~(FTM_SC_CLKS_MASK);
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM2_SC) = regval;
 
-	/* Enabled System Clock Gating Control for FTM0, and FTM2 */
+	/* Enabled System Clock Gating Control for FTM0, FTM1 and FTM2 */
 
 	regval = _REG(S32K1XX_PCC_FTM0);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM0) = regval;
 
+	regval = _REG(S32K1XX_PCC_FTM1);
+	regval |= PCC_CGC;
+	_REG(S32K1XX_PCC_FTM1) = regval;
+
 	regval = _REG(S32K1XX_PCC_FTM2);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM2) = regval;

boards/px4/fmu-v6x/default.px4board

@@ -74,6 +74,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y

msg/ControlAllocatorStatus.msg

@@ -1,12 +1,10 @@
 uint64 timestamp                        # time since system start (microseconds)
 
 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_torque = torque_setpoint - allocated_torque
 
 bool thrust_setpoint_achieved           # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust
 

msg/VehicleAttitudeSetpoint.msg

@@ -13,7 +13,7 @@ float32[4] q_d			# Desired quaternion for quaternion control
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]
 
-bool reset_rate_integrals	# Reset roll/pitch/yaw integrals (navigation logic change)
+bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)
 
 bool fw_control_yaw		# control heading with rudder (used for auto takeoff on runway)
 

msg/VehicleLandDetected.msg

@@ -12,6 +12,7 @@ bool has_low_throttle
 
 bool vertical_movement
 bool horizontal_movement
+bool rotational_movement
 
 bool close_to_ground_or_skipped_check
 

msg/VehicleRatesSetpoint.msg

@@ -8,3 +8,5 @@ float32 yaw		# [rad/s] yaw rate setpoint
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body	# Normalized thrust command in body NED frame [-1,1]
+
+bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change)

platforms/common/include/px4_platform_common/defines.h

@@ -136,5 +136,9 @@ __END_DECLS
 #define M_LOG2_E_F		0.69314718f
 #define M_INVLN2_F		1.44269504f	// 1 / log(2)
 
+/* The M_PI, as stated above, is not C standard. If you need it and
+ * it isn't in your math.h file then you can use this instead. */
+#define M_PI_PRECISE	3.141592653589793238462643383279502884
+
 #define M_DEG_TO_RAD 		0.017453292519943295
 #define M_RAD_TO_DEG 		57.295779513082323

platforms/common/spi.cpp

@@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
 #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
 #else
-	int hw_version_revision = (board_get_hw_version() << 16) | board_get_hw_revision();
+	int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
 #endif
 
 

platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h

@@ -45,8 +45,8 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			1
-#define MAX_TIMER_IO_CHANNELS		1
+#define MAX_IO_TIMERS			2
+#define MAX_TIMER_IO_CHANNELS		2
 
 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3

platforms/qurt/CMakeLists.txt

@@ -38,4 +38,5 @@ QURT_LIB(LIB_NAME px4
 		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
 	LINK_LIBS
 		${module_libraries}
+		px4_layer
 	)

platforms/qurt/cmake/px4_impl_os.cmake

@@ -126,6 +126,7 @@ function(px4_os_add_flags)
 
 		-Wno-unknown-warning-option
 		-Wno-cast-align
+		--include=${PX4_SOURCE_DIR}/platforms/qurt/include/qurt_reqs.h
 	)
 
 	# Clear -rdynamic flag which fails for hexagon

platforms/qurt/include/px4_arch/micro_hal.h

@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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.
+ *
+ ****************************************************************************/
+
+// Placeholder
+

platforms/qurt/include/qurt_reqs.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies found in PX4
+// that have not been implemented in the Hexagon SDK yet.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef unsigned long useconds_t;
+int usleep(useconds_t usec);
+
+#ifdef __cplusplus
+}
+#endif

platforms/qurt/include/sys/ioctl.h

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies on IOCTL found in PX4.
+// As QURT does not have IOCTL natively, this file exists to define those
+// functions/params found throughout the code base.
+
+#pragma once
+
+#define	IOCPARM_MASK	0x1fff		/* parameter length, at most 13 bits */
+#define	IOC_VOID	0x20000000	/* no parameters */
+
+#define	_IOC(inout,group,num,len)	((unsigned long) \
+		((inout) | (((len) & IOCPARM_MASK) << 16) | ((group) << 8) | (num)))
+#define	_IO(g,n)	_IOC(IOC_VOID,	(g), (n), 0)

platforms/qurt/src/px4/CMakeLists.txt

@@ -32,3 +32,13 @@
 ############################################################################
 
 # Placeholder
+set(QURT_LAYER_SRCS
+	drv_hrt.cpp
+	tasks.cpp
+	)
+
+add_library(px4_layer
+        ${QURT_LAYER_SRCS}
+)
+
+target_link_libraries(px4_layer PRIVATE work_queue)

platforms/qurt/src/px4/drv_hrt.cpp

@@ -0,0 +1,326 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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 <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/workqueue.h>
+#include <drivers/drv_hrt.h>
+
+#include <semaphore.h>
+#include <time.h>
+#include <string.h>
+#include <errno.h>
+
+#include "hrt_work.h"
+
+static constexpr unsigned HRT_INTERVAL_MIN = 50;
+static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
+
+static struct sq_queue_s	callout_queue;
+
+static uint64_t			latency_baseline;
+
+static uint64_t			latency_actual;
+
+const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+static px4_sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+static int32_t dsp_offset = 0;
+
+static void hrt_latency_update();
+
+static void hrt_call_reschedule();
+static void hrt_call_invoke();
+
+hrt_abstime hrt_absolute_time_offset()
+{
+	return 0;
+}
+
+static void hrt_lock()
+{
+	px4_sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock()
+{
+	px4_sem_post(&_hrt_lock);
+}
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	return 0;
+}
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	int rv = clock_gettime(clk_id, tp);
+	hrt_abstime temp_abstime = ts_to_abstime(tp);
+
+	if (dsp_offset < 0) {
+		hrt_abstime temp_offset = -dsp_offset;
+
+		if (temp_offset >= temp_abstime) { temp_abstime = 0; }
+
+		else { temp_abstime -= temp_offset; }
+
+	} else {
+		temp_abstime += (hrt_abstime) dsp_offset;
+	}
+
+	tp->tv_sec = temp_abstime / 1000000;
+	tp->tv_nsec = (temp_abstime % 1000000) * 1000;
+	return rv;
+}
+
+hrt_abstime hrt_absolute_time()
+{
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+}
+
+int hrt_set_absolute_time_offset(int32_t time_diff_us)
+{
+	dsp_offset = time_diff_us;
+	return 0;
+}
+
+hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
+}
+
+void hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	*t = hrt_absolute_time();
+}
+
+bool	hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+void	hrt_cancel(struct hrt_call *entry)
+{
+	hrt_lock();
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+	entry->period = 0;
+	hrt_unlock();
+}
+
+static void hrt_latency_update()
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	latency_counters[index]++;
+}
+
+void	hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
+
+void	hrt_init()
+{
+	sq_init(&callout_queue);
+
+	int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
+
+	if (sem_ret) {
+		PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
+	}
+
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == nullptr) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == nullptr) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != nullptr);
+	}
+}
+
+static void
+hrt_tim_isr(void *p)
+{
+	latency_actual = hrt_absolute_time();
+	hrt_latency_update();
+	hrt_call_invoke();
+	hrt_lock();
+	hrt_call_reschedule();
+	hrt_unlock();
+}
+
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	hrt_abstime	delay = HRT_INTERVAL_MAX;
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	if (next != nullptr) {
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			delay = HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			delay = next->deadline - now;
+		}
+	}
+
+	latency_baseline = now + delay;
+	hrt_work_cancel(&_hrt_work);
+	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
+	hrt_lock();
+
+	if (entry->deadline != 0) {
+		sq_rem(&entry->link, &callout_queue);
+	}
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+	hrt_unlock();
+}
+
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+static void
+hrt_call_invoke()
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	hrt_lock();
+
+	while (true) {
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == nullptr) {
+			break;
+		}
+
+		if (call->deadline > now) {
+			break;
+		}
+
+		sq_rem(&call->link, &callout_queue);
+		deadline = call->deadline;
+		call->deadline = 0;
+
+		if (call->callout) {
+			hrt_unlock();
+			call->callout(call->arg);
+			hrt_lock();
+		}
+
+		if (call->period != 0) {
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+
+	hrt_unlock();
+}

platforms/qurt/src/px4/tasks.cpp

@@ -0,0 +1,379 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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 <px4_platform_common/log.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>
+
+#include <drivers/drv_hrt.h>
+#include <pthread.h>
+#include "hrt_work.h"
+
+#define PX4_TASK_STACK_SIZE 8192
+#define PX4_TASK_MAX_NAME_LENGTH 32
+#define PX4_TASK_MAX_ARGC 32
+#define PX4_TASK_MAX_ARGV_LENGTH 32
+#define PX4_MAX_TASKS 24
+
+typedef struct task_entry {
+	pthread_t tid;
+	char name[PX4_TASK_MAX_NAME_LENGTH + 4];
+	char stack[PX4_TASK_STACK_SIZE];
+	pthread_attr_t attr;
+	px4_main_t main_entry;
+	int argc;
+	char argv_storage[PX4_TASK_MAX_ARGC][PX4_TASK_MAX_ARGV_LENGTH];
+	char *argv[PX4_TASK_MAX_ARGC];
+	bool isused;
+
+	task_entry() : isused(false) {}
+} task_entry_t;
+
+static task_entry_t taskmap[PX4_MAX_TASKS];
+
+static bool task_mutex_initialized = false;
+static pthread_mutex_t task_mutex;
+
+static void *entry_adapter(void *ptr)
+{
+	task_entry_t *data;
+	data = (task_entry_t *) ptr;
+
+	if (data->main_entry) { data->main_entry(data->argc, data->argv); }
+
+	else { PX4_ERR("No valid task entry points"); }
+
+	pthread_exit(nullptr);
+	return nullptr;
+
+}
+
+static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_main_t main_entry, char *const argv[])
+{
+	int retcode = 0;
+	int i = 0;
+	int task_index = 0;
+	char *p = (char *)argv;
+
+	PX4_INFO("Creating pthread %s\n", name);
+
+	if (task_mutex_initialized == false) {
+		task_mutex_initialized = true;
+		pthread_mutex_init(&task_mutex, nullptr);
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (task_index = 0; task_index < PX4_MAX_TASKS; task_index++) {
+		if (taskmap[task_index].isused == false) { break; }
+	}
+
+	if (task_index == PX4_MAX_TASKS) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Hit maximum number of threads");
+		return -1;
+	}
+
+	taskmap[task_index].argc = 0;
+
+	while (p) {
+		taskmap[task_index].argc++;
+		p = argv[taskmap[task_index].argc];
+	}
+
+	if (taskmap[task_index].argc >= PX4_TASK_MAX_ARGC) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Too many arguments for thread %d", taskmap[task_index].argc);
+		return -1;
+	}
+
+	for (i = 0; i < PX4_TASK_MAX_ARGC; i++) {
+		if (i < taskmap[task_index].argc) {
+			int argument_length = strlen(argv[i]);
+
+			if (argument_length >= PX4_TASK_MAX_ARGV_LENGTH) {
+				pthread_mutex_unlock(&task_mutex);
+				PX4_ERR("Argument %d is too long %d", i, argument_length);
+				return -1;
+
+			} else {
+				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
+				taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
+			}
+
+		} else {
+			taskmap[task_index].argv[i] = nullptr;
+			break;
+		}
+	}
+
+	taskmap[task_index].main_entry = main_entry;
+
+	if ((priority > 255) || (priority < 0)) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Invalid priority %d", priority);
+		return -1;
+	}
+
+	priority = 255 - priority;
+
+	if (priority < 5) { priority = 5; }
+
+	if (priority > 250) { priority = 250; }
+
+	if (strlen(name) >= PX4_TASK_MAX_NAME_LENGTH) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Task name is too long %s", name);
+		return -1;
+	}
+
+	strcpy(taskmap[task_index].name, "PX4_");
+	strcpy(&taskmap[task_index].name[4], name);
+
+	struct sched_param param;
+	param.sched_priority = priority;
+
+	pthread_attr_init(&taskmap[task_index].attr);
+	//pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
+	//pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
+	pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE);
+	pthread_attr_setschedparam(&taskmap[task_index].attr, &param);
+
+	retcode = pthread_create(&taskmap[task_index].tid, &taskmap[task_index].attr, entry_adapter,
+				 (void *) &taskmap[task_index]);
+
+	if (retcode != PX4_OK) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Couldn't create pthread %s", name);
+		return -1;
+
+	} else {
+		PX4_INFO("Successfully created px4 task %s with tid %u",
+			 taskmap[task_index].name,
+			 (unsigned int) taskmap[task_index].tid);
+	}
+
+	taskmap[task_index].isused = true;
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return i;
+}
+
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
+			      char *const argv[])
+{
+	if (entry == nullptr) {
+		PX4_ERR("Entry function pointer is null");
+		return -1;
+	}
+
+	return px4_task_spawn_internal(name, priority, entry, argv);
+}
+
+int px4_task_delete(px4_task_t id)
+{
+	int rv = 0;
+
+	PX4_ERR("Ignoring px4_task_delete for task %d", id);
+
+	pthread_t pid;
+	PX4_WARN("Called px4_task_delete");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
+		pid = taskmap[id].tid;
+
+	} else {
+		return -EINVAL;
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	if (pthread_self() == pid) {
+		pthread_join(pid, nullptr);
+		taskmap[id].isused = false;
+		pthread_mutex_unlock(&task_mutex);
+		pthread_exit(nullptr);
+
+	} else {
+		rv = pthread_cancel(pid);
+	}
+
+	taskmap[id].isused = false;
+
+	return rv;
+}
+
+void px4_task_exit(int ret)
+{
+	PX4_ERR("Ignoring px4_task_exit with return value %d", ret);
+
+	int i;
+	pthread_t pid = pthread_self();
+
+	for (i = 0; i < PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].tid == pid) {
+			pthread_mutex_lock(&task_mutex);
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+
+	if (i >= PX4_MAX_TASKS)  {
+		PX4_ERR("px4_task_exit: self task not found!");
+
+	} else {
+		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name);
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name);
+
+	if (id < PX4_MAX_TASKS && taskmap[id].tid != 0) {
+		pthread_mutex_lock(&task_mutex);
+		pid = taskmap[id].tid;
+		pthread_mutex_unlock(&task_mutex);
+
+	} else {
+		return -EINVAL;
+	}
+
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}
+
+void px4_show_tasks()
+{
+	int idx = 0;
+	int count = 0;
+
+	PX4_INFO("Active Tasks:");
+
+	for (; idx < PX4_MAX_TASKS; idx++) {
+		if (taskmap[idx].isused) {
+			PX4_INFO("   %-10s %u", taskmap[idx].name,
+				 (unsigned int) taskmap[idx].tid);
+			count++;
+		}
+	}
+
+	if (count == 0) {
+		PX4_INFO("No running tasks");
+	}
+}
+
+px4_task_t px4_getpid()
+{
+	pthread_t pid = pthread_self();
+	px4_task_t ret = -1;
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			ret = i;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+	return ret;
+
+}
+
+
+const char *px4_get_taskname()
+{
+	pthread_t pid = pthread_self();
+	const char *prog_name = "UnknownApp";
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			prog_name = taskmap[i].name;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return prog_name;
+
+}
+
+static void timer_cb(void *data)
+{
+	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+
+	sem_post(sem);
+}
+
+int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
+{
+	work_s _hpwork = {};
+
+	struct timespec ts_now;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
+
+	hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
+
+	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
+	sem_wait(sem);
+	hrt_work_cancel(&_hpwork);
+	return 0;
+}
+
+int px4_prctl(int option, const char *arg2, pthread_t pid)
+{
+	int rv = -1;
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
+			return rv;
+		}
+	}
+
+	return rv;
+}

src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp

@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstdlib>
 
 namespace InvenSense_ICM20649
 {

src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp

@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstdlib>
 
 namespace InvenSense_ICM42670P
 {

src/drivers/magnetometer/lis2mdl/lis2mdl.h

@@ -80,8 +80,8 @@
 #define CFG_REG_C_BDU                   (1 << 4) /* avoids reading of incorrect data due to async reads */
 
 /* interface factories */
-extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 #define LIS2MDLL_ADDRESS        0x1e
 

src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp

@@ -56,7 +56,7 @@
 class LIS2MDL_I2C : public device::I2C
 {
 public:
-	LIS2MDL_I2C(int bus, int bus_frequency);
+	LIS2MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_I2C() = default;
 
 	virtual int     read(unsigned address, void *data, unsigned count);
@@ -68,16 +68,16 @@ protected:
 };
 
 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency);
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency)
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_I2C(bus, bus_frequency);
+	return new LIS2MDL_I2C(config);
 }
 
-LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency)
+LIS2MDL_I2C::LIS2MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }
 

src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp

@@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS2MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;
 
 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS2MDL_I2C_interface(config);
 
 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS2MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS2MDL_SPI_interface(config);
 	}
 
 	if (interface == nullptr) {
@@ -94,6 +94,7 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS2MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS2MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;
 
@@ -112,8 +113,6 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 		return -1;
 	}
 
-	cli.i2c_address = LIS2MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL);
 
 	if (!strcmp(verb, "start")) {

src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp

@@ -61,7 +61,7 @@
 class LIS2MDL_SPI : public device::SPI
 {
 public:
-	LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS2MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_SPI() = default;
 
 	virtual int     init();
@@ -70,16 +70,16 @@ public:
 };
 
 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS2MDL_SPI(config);
 }
 
-LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency)
+LIS2MDL_SPI::LIS2MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }
 

src/drivers/magnetometer/lis3mdl/lis3mdl.h

@@ -84,8 +84,8 @@
 #define CNTL_REG5_DEFAULT 0x00
 
 /* interface factories */
-extern device::Device *LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS3MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 enum OPERATING_MODE {
 	CONTINUOUS = 0,

src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp

@@ -56,7 +56,7 @@
 class LIS3MDL_I2C : public device::I2C
 {
 public:
-	LIS3MDL_I2C(int bus, int bus_frequency);
+	LIS3MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_I2C() = default;
 
 	virtual int     read(unsigned address, void *data, unsigned count);
@@ -68,16 +68,16 @@ protected:
 };
 
 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency);
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency)
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_I2C(bus, bus_frequency);
+	return new LIS3MDL_I2C(config);
 }
 
-LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, LIS3MDLL_ADDRESS, bus_frequency)
+LIS3MDL_I2C::LIS3MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }
 

src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp

@@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS3MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;
 
 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS3MDL_I2C_interface(config);
 
 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS3MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS3MDL_SPI_interface(config);
 	}
 
 	if (interface == nullptr) {
@@ -90,6 +90,7 @@ void LIS3MDL::print_usage()
 	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1e);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
 	PRINT_MODULE_USAGE_COMMAND("reset");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
@@ -100,6 +101,7 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS3MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS3MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;
 
@@ -118,8 +120,6 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 		return -1;
 	}
 
-	cli.i2c_address = LIS3MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS3MDL);
 
 	if (!strcmp(verb, "start")) {

src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp

@@ -61,7 +61,7 @@
 class LIS3MDL_SPI : public device::SPI
 {
 public:
-	LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS3MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_SPI() = default;
 
 	virtual int     init();
@@ -70,16 +70,16 @@ public:
 };
 
 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS3MDL_SPI(config);
 }
 
-LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, devid, spi_mode, bus_frequency)
+LIS3MDL_SPI::LIS3MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }
 

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -121,7 +121,7 @@ MspOsd::MspOsd(const char *device) :
 	strcpy(_device, device);
 
 	// _is_initialized = true;
-	PX4_INFO("MSP OSD prepared to run on %s", _device);
+	PX4_INFO("MSP OSD running on %s", _device);
 }
 
 MspOsd::~MspOsd()
@@ -504,10 +504,10 @@ int MspOsd::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Msp OSD!
+MSP telemetry streamer
 
 ### Implementation
-Does the things for the DJI Air Unit OSD
+Converts uORB messages to MSP telemetry packets
 
 ### Examples
 CLI usage example:

src/lib/matrix/matrix/Euler.hpp

@@ -93,11 +93,11 @@ public:
 	{
 		theta() = std::asin(-dcm(2, 0));
 
-		if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
+		if ((std::fabs(theta() - Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(dcm(1, 2), dcm(0, 2));
 
-		} else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
+		} else if ((std::fabs(theta() + Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(-dcm(1, 2), -dcm(0, 2));
 

src/lib/matrix/matrix/helper_functions.hpp

@@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high)
 template<typename Type>
 Type wrap_pi(Type x)
 {
-	return wrap(x, Type(-M_PI), Type(M_PI));
+	return wrap(x, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }
 
 /**
@@ -104,7 +104,7 @@ Type wrap_pi(Type x)
 template<typename Type>
 Type wrap_2pi(Type x)
 {
-	return wrap(x, Type(0), Type((2 * M_PI)));
+	return wrap(x, Type(0), Type((2 * M_PI_PRECISE)));
 }
 
 /**
@@ -132,7 +132,7 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high
 template<typename Type>
 Type unwrap_pi(const Type last_angle, const Type new_angle)
 {
-	return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI));
+	return unwrap(last_angle, new_angle, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }
 
 /**

src/lib/matrix/matrix/math.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <assert.h>
+#include <px4_platform_common/defines.h>
 
 #include "helper_functions.hpp"
 

src/lib/matrix/test/MatrixAttitudeTest.cpp

@@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude)
 	}
 
 	// constants
-	double deg2rad = M_PI / 180.0;
-	double rad2deg = 180.0 / M_PI;
+	double deg2rad = M_PI_PRECISE / 180.0;
+	double rad2deg = 180.0 / M_PI_PRECISE;
 
 	// euler dcm round trip check
 	for (double roll = -90; roll <= 90; roll += 90) {
@@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude)
 	EXPECT_EQ(q, q_true);
 
 	// from axis angle, with length of vector the rotation
-	float n = float(std::sqrt(4 * M_PI * M_PI / 3));
+	float n = float(std::sqrt(4 * M_PI_F * M_PI_F / 3));
 	q = AxisAnglef(n, n, n);
 	EXPECT_EQ(q, Quatf(-1, 0, 0, 0));
 	q = AxisAnglef(0, 0, 0);

src/lib/matrix/test/MatrixHelperTest.cpp

@@ -81,20 +81,20 @@ TEST(MatrixHelperTest, Helper)
 
 	// wrap pi
 	EXPECT_FLOAT_EQ(wrap_pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE)));
 	EXPECT_FLOAT_EQ(wrap_pi(3.), 3.);
-	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE));
 	EXPECT_FALSE(std::isfinite(wrap_pi(NAN)));
 
 	// wrap 2pi
 	EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE));
 	EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.));
-	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE)));
 	EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));
 
 	// Equality checks

src/lib/matrix/test/MatrixUnwrapTest.cpp

@@ -5,7 +5,7 @@ using namespace matrix;
 
 TEST(MatrixUnwrapTest, UnwrapFloats)
 {
-	const float M_TWO_PI_F = float(M_PI * 2);
+	const float M_TWO_PI_F = float(M_PI_F * 2);
 
 	float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
@@ -34,7 +34,7 @@ TEST(MatrixUnwrapTest, UnwrapFloats)
 
 TEST(MatrixUnwrapTest, UnwrapDoubles)
 {
-	const double M_TWO_PI = M_PI * 2;
+	const double M_TWO_PI = M_PI_PRECISE * 2;
 
 	double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};

src/lib/parameters/param_translation.cpp

@@ -250,5 +250,14 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y
+	{
+		if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) {
+			strcpy(node->name, "VT_FW_DIFTHR_S_Y");
+			PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y");
+			return true;
+		}
+	}
+
 	return false;
 }

src/lib/perf/perf_counter.cpp

@@ -419,35 +419,25 @@ perf_print_counter(perf_counter_t handle)
 		return;
 	}
 
-	perf_print_counter_fd(1, handle);
-}
-
-void
-perf_print_counter_fd(int fd, perf_counter_t handle)
-{
-	if (handle == nullptr) {
-		return;
-	}
-
 	switch (handle->type) {
 	case PC_COUNT:
-		dprintf(fd, "%s: %" PRIu64 " events\n",
-			handle->name,
-			((struct perf_ctr_count *)handle)->event_count);
+		PX4_INFO_RAW("%s: %" PRIu64 " events\n",
+			     handle->name,
+			     ((struct perf_ctr_count *)handle)->event_count);
 		break;
 
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 			float rms = sqrtf(pce->M2 / (pce->event_count - 1));
-			dprintf(fd, "%s: %" PRIu64 " events, %" PRIu64 "us elapsed, %.2fus avg, min %" PRIu32 "us max %" PRIu32
-				"us %5.3fus rms\n",
-				handle->name,
-				pce->event_count,
-				pce->time_total,
-				(pce->event_count == 0) ? 0 : (double)pce->time_total / (double)pce->event_count,
-				pce->time_least,
-				pce->time_most,
-				(double)(1e6f * rms));
+			PX4_INFO_RAW("%s: %" PRIu64 " events, %" PRIu64 "us elapsed, %.2fus avg, min %" PRIu32 "us max %" PRIu32
+				     "us %5.3fus rms\n",
+				     handle->name,
+				     pce->event_count,
+				     pce->time_total,
+				     (pce->event_count == 0) ? 0 : (double)pce->time_total / (double)pce->event_count,
+				     pce->time_least,
+				     pce->time_most,
+				     (double)(1e6f * rms));
 			break;
 		}
 
@@ -455,13 +445,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 			float rms = sqrtf(pci->M2 / (pci->event_count - 1));
 
-			dprintf(fd, "%s: %" PRIu64 " events, %.2fus avg, min %" PRIu32 "us max %" PRIu32 "us %5.3fus rms\n",
-				handle->name,
-				pci->event_count,
-				(pci->event_count == 0) ? 0 : (double)(pci->time_last - pci->time_first) / (double)pci->event_count,
-				pci->time_least,
-				pci->time_most,
-				(double)(1e6f * rms));
+			PX4_INFO_RAW("%s: %" PRIu64 " events, %.2fus avg, min %" PRIu32 "us max %" PRIu32 "us %5.3fus rms\n",
+				     handle->name,
+				     pci->event_count,
+				     (pci->event_count == 0) ? 0 : (double)(pci->time_last - pci->time_first) / (double)pci->event_count,
+				     pci->time_least,
+				     pci->time_most,
+				     (double)(1e6f * rms));
 			break;
 		}
 
@@ -593,13 +583,13 @@ perf_iterate_all(perf_callback cb, void *user)
 }
 
 void
-perf_print_all(int fd)
+perf_print_all(void)
 {
 	pthread_mutex_lock(&perf_counters_mutex);
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != nullptr) {
-		perf_print_counter_fd(fd, handle);
+		perf_print_counter(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
 
@@ -607,19 +597,19 @@ perf_print_all(int fd)
 }
 
 void
-perf_print_latency(int fd)
+perf_print_latency(void)
 {
 	latency_info_t latency;
-	dprintf(fd, "bucket [us] : events\n");
+	PX4_INFO_RAW("bucket [us] : events\n");
 
 	for (int i = 0; i < get_latency_bucket_count(); i++) {
 		latency = get_latency(i, i);
-		dprintf(fd, "       %4i : %li\n", latency.bucket, (long int)latency.counter);
+		PX4_INFO_RAW("       %4i : %li\n", latency.bucket, (long int)latency.counter);
 	}
 
 	// print the overflow bucket value
 	latency = get_latency(get_latency_bucket_count() - 1, get_latency_bucket_count());
-	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
+	PX4_INFO_RAW(" >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
 }
 
 void

src/lib/perf/perf_counter.h

@@ -174,14 +174,6 @@ __EXPORT extern void		perf_reset(perf_counter_t handle);
  */
 __EXPORT extern void		perf_print_counter(perf_counter_t handle);
 
-/**
- * Print one performance counter to a fd.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
- * @param handle		The counter to print.
- */
-__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
-
 /**
  * Print one performance counter to a buffer.
  *
@@ -194,10 +186,8 @@ __EXPORT extern int		perf_print_counter_buffer(char *buffer, int length, perf_co
 
 /**
  * Print all of the performance counters.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_all(int fd);
+__EXPORT extern void		perf_print_all(void);
 
 
 typedef void (*perf_callback)(perf_counter_t handle, void *user);
@@ -216,10 +206,8 @@ __EXPORT extern void	perf_iterate_all(perf_callback cb, void *user);
 
 /**
  * Print hrt latency counters.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_latency(int fd);
+__EXPORT extern void		perf_print_latency(void);
 
 /**
  * Reset all of the performance counters.

src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -218,10 +218,10 @@ AngularVelocityController::Run()
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			Vector3f integral = _control.getIntegral();
-			rate_ctrl_status.timestamp = hrt_absolute_time();
 			rate_ctrl_status.rollspeed_integ = integral(0);
 			rate_ctrl_status.pitchspeed_integ = integral(1);
 			rate_ctrl_status.yawspeed_integ = integral(2);
+			rate_ctrl_status.timestamp = hrt_absolute_time();
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);
 
 			// publish controller output
@@ -238,14 +238,12 @@ void
 AngularVelocityController::publish_angular_acceleration_setpoint()
 {
 	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
-
 	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
-	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
 	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
 	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
 	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
-
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
 }
 
@@ -253,14 +251,12 @@ void
 AngularVelocityController::publish_torque_setpoint()
 {
 	Vector3f torque_sp = _control.getTorqueSetpoint();
-
 	vehicle_torque_setpoint_s v_torque_sp = {};
-	v_torque_sp.timestamp = hrt_absolute_time();
 	v_torque_sp.timestamp_sample = _timestamp_sample;
 	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
 	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
 	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+	v_torque_sp.timestamp = hrt_absolute_time();
 	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
 }
 
@@ -268,12 +264,11 @@ void
 AngularVelocityController::publish_thrust_setpoint()
 {
 	vehicle_thrust_setpoint_s v_thrust_sp = {};
-	v_thrust_sp.timestamp = hrt_absolute_time();
 	v_thrust_sp.timestamp_sample = _timestamp_sample;
 	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
 	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
 	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
-
+	v_thrust_sp.timestamp = hrt_absolute_time();
 	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
 }
 

src/modules/commander/Commander.cpp

@@ -1575,7 +1575,7 @@ void Commander::updateParameters()
 			       && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 	const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status)
 			      && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-	const bool is_ground = is_ground_rover(_vehicle_status);
+	const bool is_ground = is_ground_vehicle(_vehicle_status);
 
 	/* disable manual override for all systems that rely on electronic stabilization */
 	if (is_rotary) {

src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp

@@ -35,11 +35,6 @@
 
 using namespace time_literals;
 
-OffboardChecks::OffboardChecks()
-{
-	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
-}
-
 void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	reporter.failsafeFlags().offboard_control_signal_lost = true;
@@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 	offboard_control_mode_s offboard_control_mode;
 
 	if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
-		bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
-					  || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
-					  || offboard_control_mode.actuator;
+		bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s;
+		bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity
+					   || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
+					   || offboard_control_mode.actuator) && data_is_recent;
 
 		if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
 			offboard_available = false;
@@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 			offboard_available = false;
 		}
 
-		_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
-
 		// This is a mode requirement, no need to report
-		reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
+		reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available;
 	}
 }

src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp

@@ -37,19 +37,17 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/offboard_control_mode.h>
-#include <lib/hysteresis/hysteresis.h>
 
 class OffboardChecks : public HealthAndArmingCheckBase
 {
 public:
-	OffboardChecks();
+	OffboardChecks() = default;
 	~OffboardChecks() = default;
 
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
 	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	systemlib::Hysteresis _offboard_available{false};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t

src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp

@@ -119,7 +119,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
 					    events::ID("check_rc_dl_no_dllink"),
 					    log_level, "No connection to the ground control station");
 
-		if (reporter.mavlink_log_pub()) {
+		if (gcs_connection_required && reporter.mavlink_log_pub()) {
 			mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t");
 		}
 

src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp

@@ -32,6 +32,8 @@
  ****************************************************************************/
 
 #include "sdcardCheck.hpp"
+#include <dirent.h>
+#include <string.h>
 
 #ifdef __PX4_DARWIN
 #include <sys/param.h>
@@ -42,38 +44,85 @@
 
 void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_arm_sdcard.get() <= 0) {
-		return;
-	}
+	if (_param_com_arm_sdcard.get() > 0) {
 
-	struct statfs statfs_buf;
+		struct statfs statfs_buf;
 
-	if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
-		// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
-		_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
-	}
-
-	if (!_sdcard_detected) {
-		NavModes affected_modes{NavModes::None};
-
-		if (_param_com_arm_sdcard.get() == 2) {
-			// disallow arming without sd card
-			affected_modes = NavModes::All;
+		if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
+			// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
+			_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
 		}
 
+		if (!_sdcard_detected) {
+			NavModes affected_modes{NavModes::None};
+
+			if (_param_com_arm_sdcard.get() == 2) {
+				// disallow arming without sd card
+				affected_modes = NavModes::All;
+			}
+
+			/* EVENT
+			 * @description
+			 * Insert an SD Card to the autopilot and reboot the system.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(affected_modes, health_component_t::system,
+						    events::ID("check_missing_fmu_sdcard"),
+						    events::Log::Error, "Missing FMU SD Card");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			}
+		}
+	}
+
+#ifdef __PX4_NUTTX
+	// Check for hardfault files
+
+	if (!_hardfault_checked_once && _param_com_arm_hardfault_check.get()) {
+		_hardfault_checked_once = true;
+
+		DIR *dp = opendir(PX4_STORAGEDIR);
+
+		if (dp != nullptr) {
+
+			struct dirent *result;
+
+			while ((result = readdir(dp)) && !_hardfault_file_present) {
+
+				// Check for pattern fault_*.log
+				if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+					_hardfault_file_present = true;
+				}
+			}
+
+			closedir(dp);
+		}
+	}
+
+	if (_hardfault_file_present && _param_com_arm_hardfault_check.get()) {
 		/* EVENT
 		 * @description
-		 * Insert an SD Card to the autopilot and reboot the system.
+		 * The SD card contains crash dump files, service the vehicle before continuing to fly.
 		 *
 		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+		 * Check how to debug hardfaults on <a>https://docs.px4.io/main/en/debug/gdb_debugging.html#hard-fault-debugging</a>.
+		 * When completed, remove the files 'fault_*.log' on the SD card.
+		 *
+		 * This check can be configured via <param>COM_ARM_HFLT_CHK</param> parameter.
 		 * </profile>
 		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"),
-					    events::Log::Error, "Missing FMU SD Card");
+		reporter.healthFailure(NavModes::All, health_component_t::system,
+				       events::ID("check_hardfault_present"),
+				       events::Log::Error, "Crash dumps present on SD card");
 
 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service");
 		}
 	}
+
+#endif /* __PX4_NUTTX */
 }

src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp

@@ -46,7 +46,13 @@ public:
 private:
 	bool _sdcard_detected{false};
 
+#ifdef __PX4_NUTTX
+	bool _hardfault_checked_once {false};
+	bool _hardfault_file_present{false};
+#endif
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard
+					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard,
+					(ParamBool<px4::params::COM_ARM_HFLT_CHK>) _param_com_arm_hardfault_check
 				       )
 };

src/modules/commander/commander_helper.cpp

@@ -118,19 +118,9 @@ bool is_fixed_wing(const vehicle_status_s &current_status)
 	return current_status.system_type == VEHICLE_TYPE_FIXED_WING;
 }
 
-bool is_ground_rover(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
-}
-
-bool is_boat(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_BOAT;
-}
-
 bool is_ground_vehicle(const vehicle_status_s &current_status)
 {
-	return is_ground_rover(current_status) || is_boat(current_status);
+	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }
 
 // End time for currently blinking LED message, 0 if no blink message

src/modules/commander/commander_helper.h

@@ -55,8 +55,6 @@ bool is_rotary_wing(const vehicle_status_s &current_status);
 bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
-bool is_ground_rover(const vehicle_status_s &current_status);
-bool is_boat(const vehicle_status_s &current_status);
 bool is_ground_vehicle(const vehicle_status_s &current_status);
 
 int buzzer_init(void);

src/modules/commander/commander_params.c

@@ -1010,6 +1010,17 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
 
+/**
+ * Enable FMU SD card hardfault detection check
+ *
+ * This check detects if there are hardfault files present on the
+ * SD card. If so, and the parameter is enabled, arming is prevented.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
+
 /**
  * Enforced delay between arming and further navigation
  *

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -199,10 +199,10 @@ public:
 	virtual uint32_t getStoppedMotors() const { return 0; }
 
 	/**
-	 * Fill in the allocated and unallocated torque and thrust.
-	 * Should return false if not filled in and the effectivenes matrix should be used instead
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
 	 */
-	virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; }
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
 
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -217,48 +217,36 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit
 	}
 }
 
-bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const
+void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
 {
-	status.torque_setpoint_achieved = true;
-	status.thrust_setpoint_achieved = true;
 
 	// Note: the values '-1', '1' and '0' are just to indicate a negative,
 	// positive or no saturation to the rate controller. The actual magnitude is not used.
 	if (_saturation_flags.roll_pos) {
 		status.unallocated_torque[0] = 1.f;
-		status.torque_setpoint_achieved = false;
 
 	} else if (_saturation_flags.roll_neg) {
 		status.unallocated_torque[0] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}
 
 	if (_saturation_flags.pitch_pos) {
 		status.unallocated_torque[1] = 1.f;
-		status.torque_setpoint_achieved = false;
 
 	} else if (_saturation_flags.pitch_neg) {
 		status.unallocated_torque[1] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}
 
 	if (_saturation_flags.yaw_pos) {
 		status.unallocated_torque[2] = 1.f;
-		status.torque_setpoint_achieved = false;
 
 	} else if (_saturation_flags.yaw_neg) {
 		status.unallocated_torque[2] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}
 
 	if (_saturation_flags.thrust_pos) {
 		status.unallocated_thrust[2] = 1.f;
-		status.thrust_setpoint_achieved = false;
 
 	} else if (_saturation_flags.thrust_neg) {
 		status.unallocated_thrust[2] = -1.f;
-		status.thrust_setpoint_achieved = false;
 	}
-
-	return true;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -79,7 +79,7 @@ public:
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
-	bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override;
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 	return num_actuators;
 }
 
-uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
 {
-	if (!PX4_ISFINITE(tilt_control)) {
-		tilt_control = -1.f;
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
 	}
 
 	uint32_t nontilted_motors = 0;
@@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv
 		}
 
 		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
-		float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
-		float tilt_direction = math::radians((float)tilt.tilt_direction);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
 		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
 	}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -54,7 +54,7 @@ bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
-	if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+	if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
@@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
-	const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
-					   _last_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
+	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
+			-1.f : _last_collective_tilt_control;
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
@@ -86,7 +86,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 
 	// If it was an update coming from a config change, then make sure to update matrix in
 	// the next iteration again with the correct tilt (but without updating the normalization scale).
-	_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	_collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 
 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
@@ -113,24 +113,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		actuator_controls_s actuator_controls_1;
 
 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			float control_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
+			float control_collective_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
 
-			// set control_tilt to exactly -1 or 1 if close to these end points
-			control_tilt = control_tilt < -0.99f ? -1.f : control_tilt;
-			control_tilt = control_tilt > 0.99f ? 1.f : control_tilt;
+			// set control_collective_tilt to exactly -1 or 1 if close to these end points
+			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
 
-			// initialize _last_tilt_control
-			if (!PX4_ISFINITE(_last_tilt_control)) {
-				_last_tilt_control = control_tilt;
+			// initialize _last_collective_tilt_control
+			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+				_last_collective_tilt_control = control_collective_tilt;
 
-			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
-				_combined_tilt_updated = true;
-				_last_tilt_control = control_tilt;
+			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+				_collective_tilt_updated = true;
+				_last_collective_tilt_control = control_collective_tilt;
 			}
 
 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-					actuator_sp(i + _first_tilt_idx) += control_tilt;
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
 		}
@@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			}
 		}
 	}
+
+	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
+	// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
+	// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
+	if (matrix_index == 0 && _tilts.hasYawControl()) {
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
+
+		if (control_sp(2) < -1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
+
+		} else if (control_sp(2) > 1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
+		}
+	}
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
@@ -170,3 +185,23 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 		break;
 	}
 }
+
+void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// only handle matrix 0 (motors and tilts)
+	if (matrix_index == 1) {
+		return;
+	}
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -81,8 +81,11 @@ public:
 	const char *name() const override { return "VTOL Tiltrotor"; }
 
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+
 protected:
-	bool _combined_tilt_updated{true};
+	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
@@ -93,8 +96,15 @@ protected:
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0
 
-	float _last_tilt_control{NAN};
+	float _last_collective_tilt_control{NAN};
 
 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+
+	struct YawTiltSaturationFlags {
+		bool tilt_yaw_pos;
+		bool tilt_yaw_neg;
+	};
+
+	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -589,33 +589,29 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 
 	// TODO: disabled motors (?)
 
-	if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
-		// Allocated control
-		const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
-		control_allocator_status.allocated_torque[0] = allocated_control(0);
-		control_allocator_status.allocated_torque[1] = allocated_control(1);
-		control_allocator_status.allocated_torque[2] = allocated_control(2);
-		control_allocator_status.allocated_thrust[0] = allocated_control(3);
-		control_allocator_status.allocated_thrust[1] = allocated_control(4);
-		control_allocator_status.allocated_thrust[2] = allocated_control(5);
+	// Allocated control
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
 
-		// Unallocated control
-		const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
-				allocated_control;
-		control_allocator_status.unallocated_torque[0] = unallocated_control(0);
-		control_allocator_status.unallocated_torque[1] = unallocated_control(1);
-		control_allocator_status.unallocated_torque[2] = unallocated_control(2);
-		control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
-		control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
-		control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+	// Unallocated control
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
+			allocated_control;
+	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
+	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
+	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
+	control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
+	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
+	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
 
-		// Allocation success flags
-		control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
-				unallocated_control(2)).norm_squared() < 1e-6f);
-		control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
-				unallocated_control(5)).norm_squared() < 1e-6f);
-	}
+	// override control_allocator_status in customized saturation logic for certain effectiveness types
+	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);
 
+	// Allocation success flags
+	control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
+			control_allocator_status.unallocated_torque[1],
+			control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
+	control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
+			control_allocator_status.unallocated_thrust[1],
+			control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
 
 	// Actuator saturation
 	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();

src/modules/ekf2/EKF/common.h

@@ -336,6 +336,7 @@ struct parameters {
 	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
 
 	// synthetic sideslip fusion
+	int32_t beta_fusion_enabled{0};
 	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
 	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)

src/modules/ekf2/EKF/control.cpp

@@ -570,30 +570,31 @@ void Ekf::controlAirDataFusion()
 
 void Ekf::controlBetaFusion()
 {
-	if (_control_status.flags.fake_pos) {
-		return;
-	}
+	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
+		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
 
-	// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
-	const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
+	if (_control_status.flags.fuse_beta) {
 
-	if (beta_fusion_time_triggered &&
-	    _control_status.flags.fuse_beta &&
-	    _control_status.flags.in_air) {
-		updateSideslip(_aid_src_sideslip);
-		_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+		// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
+		const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
 
-		// If starting wind state estimation, reset the wind states and covariances before fusing any data
-		if (!_control_status.flags.wind) {
-			// activate the wind states
-			_control_status.flags.wind = true;
-			// reset the timeout timers to prevent repeated resets
-			_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
-			resetWind();
-		}
+		if (beta_fusion_time_triggered) {
 
-		if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
-			fuseSideslip(_aid_src_sideslip);
+			updateSideslip(_aid_src_sideslip);
+			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+
+			// If starting wind state estimation, reset the wind states and covariances before fusing any data
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the timeout timers to prevent repeated resets
+				_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
+				resetWind();
+			}
+
+			if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
+				fuseSideslip(_aid_src_sideslip);
+			}
 		}
 	}
 }

src/modules/ekf2/EKF/estimator_interface.h

@@ -132,9 +132,6 @@ public:
 	// set vehicle is fixed wing status
 	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
 
-	// set flag if synthetic sideslip measurement should be fused
-	void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
-
 	// set flag if static pressure rise due to ground effect is expected
 	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
 	// flag will clear after GNDEFFECT_TIMEOUT uSec

src/modules/ekf2/EKF2.cpp

@@ -145,6 +145,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
 	_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
 	_param_ekf2_arsp_thr(_params->arsp_thr),
+	_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
 	_param_ekf2_tau_vel(_params->vel_Tau),
 	_param_ekf2_tau_pos(_params->pos_Tau),
 	_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
@@ -532,9 +533,6 @@ void EKF2::Run()
 			if (_status_sub.copy(&vehicle_status)) {
 				const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
 
-				// only fuse synthetic sideslip measurements if conditions are met
-				_ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
-
 				// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
 				_ekf.set_is_fixed_wing(is_fixed_wing);
 

src/modules/ekf2/EKF2.hpp

@@ -569,7 +569,7 @@ private:
 		// control of airspeed and sideslip fusion
 		(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
 		_param_ekf2_arsp_thr, 	///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
-		(ParamInt<px4::params::EKF2_FUSE_BETA>)
+		(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
 		_param_ekf2_fuse_beta,		///< Controls synthetic sideslip fusion, 0 disables, 1 enables
 
 		// output predictor filter time constants

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -157,6 +157,8 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
 
+					_att_sp.reset_integral = false;
+
 					_att_sp.timestamp = hrt_absolute_time();
 
 					_attitude_sp_pub.publish(_att_sp);
@@ -390,8 +392,8 @@ void FixedwingAttitudeControl::Run()
 			const float airspeed = get_airspeed_and_update_scaling();
 
 			/* reset integrals where needed */
-			if (_att_sp.reset_rate_integrals) {
-				_rate_control.resetIntegral();
+			if (_att_sp.reset_integral) {
+				_rates_sp.reset_integral = true;
 			}
 
 			/* Reset integrators if the aircraft is on ground
@@ -401,7 +403,7 @@ void FixedwingAttitudeControl::Run()
 			    || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
 
-				_rate_control.resetIntegral();
+				_rates_sp.reset_integral = true;
 				_wheel_ctrl.reset_integrator();
 			}
 
@@ -603,7 +605,7 @@ void FixedwingAttitudeControl::Run()
 				_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
 						-1.f, 1.f) : trim_yaw;
 
-				if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
+				if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) {
 					_rate_control.resetIntegral();
 				}
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1487,7 +1487,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		if (_runway_takeoff.resetIntegrators()) {
 			// reset integrals except yaw (which also counts for the wheel controller)
-			_att_sp.reset_rate_integrals = true;
+			_att_sp.reset_integral = true;
 
 			// throttle is open loop anyway during ground roll, no need to wind up the integrator
 			_tecs.resetIntegrals();
@@ -1626,7 +1626,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		} else {
 			/* Tell the attitude controller to stop integrating while we are waiting for the launch */
-			_att_sp.reset_rate_integrals = true;
+			_att_sp.reset_integral = true;
 
 			/* Set default roll and pitch setpoints during detection phase */
 			_att_sp.roll_body = 0.0f;
@@ -2249,7 +2249,7 @@ FixedwingPositionControl::Run()
 		_npfg.setPeriod(_param_npfg_period.get());
 		_l1_control.set_l1_period(_param_fw_l1_period.get());
 
-		_att_sp.reset_rate_integrals = false;
+		_att_sp.reset_integral = false;
 
 		// by default we don't want yaw to be contoller directly with rudder
 		_att_sp.fw_control_yaw = false;

src/modules/land_detector/LandDetector.cpp

@@ -57,6 +57,7 @@ LandDetector::LandDetector() :
 	_land_detected.has_low_throttle = false;
 	_land_detected.vertical_movement = false;
 	_land_detected.horizontal_movement = false;
+	_land_detected.rotational_movement = false;
 	_land_detected.close_to_ground_or_skipped_check = true;
 	_land_detected.at_rest = true;
 }
@@ -174,6 +175,7 @@ void LandDetector::Run()
 		_land_detected.has_low_throttle = _get_has_low_throttle();
 		_land_detected.horizontal_movement = _get_horizontal_movement();
 		_land_detected.vertical_movement = _get_vertical_movement();
+		_land_detected.rotational_movement = _get_rotational_movement();
 		_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
 		_land_detected.at_rest = at_rest;
 		_land_detected.timestamp = hrt_absolute_time();

src/modules/land_detector/LandDetector.h

@@ -138,6 +138,7 @@ protected:
 	virtual bool _get_has_low_throttle() { return false; }
 	virtual bool _get_horizontal_movement() { return false; }
 	virtual bool _get_vertical_movement() { return false; }
+	virtual bool _get_rotational_movement() { return false; }
 	virtual bool _get_close_to_ground_or_skipped_check() {  return false; }
 	virtual void _set_hysteresis_factor(const int factor) = 0;
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@@ -75,12 +75,10 @@ namespace land_detector
 
 MulticopterLandDetector::MulticopterLandDetector()
 {
-	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
-	_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -119,16 +117,6 @@ void MulticopterLandDetector::_update_params()
 {
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
-	param_get(_paramHandle.landSpeed, &_params.landSpeed);
-	param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
-
-	if (_param_lndmc_z_vel_max.get() > _params.landSpeed) {
-		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f",
-			(double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed);
-
-		_param_lndmc_z_vel_max.set(_params.landSpeed);
-		_param_lndmc_z_vel_max.commit_no_notification();
-	}
 
 	int32_t use_hover_thrust_estimate = 0;
 	param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
@@ -158,22 +146,19 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 
 	const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);
 
-	// land speed threshold, 90% of MPC_LAND_SPEED
-	const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f);
-
 	if (lpos_available && _vehicle_local_position.v_z_valid) {
-		// Check if we are moving vertically - this might see a spike after arming due to
-		// throttle-up vibration. If accelerating fast the throttle thresholds will still give
-		// an accurate in-air indication.
-		float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
+		// Check if we are moving vertically.
+		// Use wider threshold if currently in "maybe landed" state, as estimation for
+		// vertical speed is often deteriorated when on the ground or due to propeller
+		// up/down throttling.
 
-		if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-			// Widen acceptance thresholds for landed state right after arming
-			// so that motor spool-up and other effects do not trigger false negatives.
-			max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
+		float vertical_velocity_threshold = _param_lndmc_z_vel_max.get();
+
+		if (_landed_hysteresis.get_state()) {
+			vertical_velocity_threshold *= 2.5f;
 		}
 
-		_vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+		_vertical_movement = (fabsf(_vehicle_local_position.vz) > vertical_velocity_threshold);
 
 	} else {
 		_vertical_movement = true;
@@ -217,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
 			// Setpoints can be NAN
 			_in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2])
-				      && (trajectory_setpoint.velocity[2] >= crawl_speed_threshold);
+				      && (trajectory_setpoint.velocity[2] >= 1.1f * _param_lndmc_z_vel_max.get());
 		}
 
 		// ground contact requires commanded descent until landed
@@ -278,21 +263,21 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 		return false;
 	}
 
+	// Next look if vehicle is not rotating (do not consider yaw)
+	float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());
 
-	float landThresholdFactor = 1.f;
-
-	// Widen acceptance thresholds for landed state right after landed
-	if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-		landThresholdFactor = 2.5f;
+	// Widen max rotation thresholds if either in landed state, thus making it harder
+	// to trigger a false positive !landed e.g. due to propeller throttling up/down.
+	if (_landed_hysteresis.get_state()) {
+		max_rotation_threshold *= 2.5f;
 	}
 
-	// Next look if all rotation angles are not moving.
-	const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
-
-	matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)};
-
-	if (angular_velocity.norm() > max_rotation_scaled) {
+	if (_angular_velocity.xy().norm() > max_rotation_threshold) {
+		_rotational_movement = true;
 		return false;
+
+	} else {
+		_rotational_movement = false;
 	}
 
 	// If vertical velocity is available: ground contact, no thrust, no movement -> landed
@@ -306,14 +291,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 
 bool MulticopterLandDetector::_get_landed_state()
 {
-	// reset the landed_time
-	if (!_maybe_landed_hysteresis.get_state()) {
-		_landed_time = 0;
-
-	} else if (_landed_time == 0) {
-		_landed_time = hrt_absolute_time();
-	}
-
 	// When not armed, consider to be landed
 	if (!_armed) {
 		return true;

src/modules/land_detector/MulticopterLandDetector.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@@ -74,6 +74,7 @@ protected:
 	bool _get_has_low_throttle() override { return _has_low_throttle; }
 	bool _get_horizontal_movement() override { return _horizontal_movement; }
 	bool _get_vertical_movement() override { return _vertical_movement; }
+	bool _get_rotational_movement() override { return _rotational_movement; }
 	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
 
 	void _set_hysteresis_factor(const int factor) override;
@@ -83,9 +84,6 @@ private:
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
 
-	/** Time interval in us in which wider acceptance thresholds are used after landed. */
-	static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
-
 	/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
 	static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;
 
@@ -94,8 +92,6 @@ private:
 		param_t minThrottle;
 		param_t hoverThrottle;
 		param_t minManThrottle;
-		param_t landSpeed;
-		param_t crawlSpeed;
 		param_t useHoverThrustEstimate;
 	} _paramHandle{};
 
@@ -103,8 +99,6 @@ private:
 		float minThrottle;
 		float hoverThrottle;
 		float minManThrottle;
-		float landSpeed;
-		float crawlSpeed;
 		bool useHoverThrustEstimate;
 	} _params{};
 
@@ -126,11 +120,11 @@ private:
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
 
 	hrt_abstime _min_thrust_start{0};	///< timestamp when minimum trust was applied first
-	hrt_abstime _landed_time{0};
 
 	bool _in_descend{false};		///< vehicle is commanded to desend
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
 	bool _vertical_movement{false};
+	bool _rotational_movement{false};
 	bool _has_low_throttle{false};
 	bool _close_to_ground_or_skipped_check{false};
 	bool _below_gnd_effect_hgt{false};	///< vehicle height above ground is below height where ground effect occurs

src/modules/land_detector/land_detector_params_mc.c

@@ -48,16 +48,19 @@
 PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);
 
 /**
- * Multicopter max climb rate
+ * Multicopter vertical velocity threshold
  *
- * Maximum vertical velocity allowed in the landed state
+ * Vertical velocity threshold to detect landing.
+ * Should be set lower than the expected minimal speed for landing
+ * so MPC_LAND_SPEED for autonomous landing and MPC_LAND_CRWL
+ * if distance sensor is present and slowing down close to ground.
  *
  * @unit m/s
  * @decimal 1
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f);
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f);
 
 /**
  * Multicopter max horizontal velocity

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -62,7 +62,7 @@ TEST(PositionControlTest, EmptySetpoint)
 	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
 	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
 	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
-	EXPECT_EQ(attitude.reset_rate_integrals, false);
+	EXPECT_EQ(attitude.reset_integral, false);
 	EXPECT_EQ(attitude.fw_control_yaw, false);
 	EXPECT_FLOAT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference?
 }

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -448,10 +448,12 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
 
 /**
- * Land crawl descend rate. Used below
+ * Land crawl descend rate
+ *
+ * Used below MPC_LAND_ALT3 if distance sensor data is availabe.
  *
  * @unit m/s
- * @min 0.3
+ * @min 0.1
  * @decimal 1
  * @group Multicopter Position Control
  */

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -296,22 +296,20 @@ MulticopterRateControl::Run()
 void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
 {
 	vehicle_torque_setpoint_s vehicle_torque_setpoint{};
-	vehicle_torque_setpoint.timestamp = hrt_absolute_time();
 	vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
 	vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
 	vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
 	vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+	vehicle_torque_setpoint.timestamp = hrt_absolute_time();
 	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
 }
 
 void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
 {
 	vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 	vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
 	_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
-
+	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
 }
 

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -30,14 +30,13 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 #include "uORBProtobufChannel.hpp"
 #include "MUORBTest.hpp"
 #include <string>
 
-#include <qurt.h>
-#include <qurt_thread.h>
 #include <pthread.h>
-
+#include <px4_platform_common/tasks.h>
 #include <px4_platform_common/log.h>
 
 // Definition of test to run when in muorb test mode
@@ -45,11 +44,11 @@ static MUORBTestType test_to_run;
 
 fc_func_ptrs muorb_func_ptrs;
 
-static void *test_runner(void *test)
+static void *test_runner(void *)
 {
 	PX4_INFO("test_runner called");
 
-	switch (*((MUORBTestType *) test)) {
+	switch (test_to_run) {
 	case ADVERTISE_TEST_TYPE:
 		(void) muorb_func_ptrs.advertise_func_ptr(muorb_test_topic_name);
 		break;
@@ -93,13 +92,13 @@ char stack[TEST_STACK_SIZE];
 
 void run_test(MUORBTestType test)
 {
-	pthread_t tid;
-	pthread_attr_t attr;
-	pthread_attr_init(&attr);
-	pthread_attr_setstacksize(&attr, TEST_STACK_SIZE);
 	test_to_run = test;
-	pthread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
-	pthread_attr_destroy(&attr);
+	(void)px4_task_spawn_cmd("test_MUORB",
+				 SCHED_DEFAULT,
+				 SCHED_PRIORITY_MAX - 2,
+				 2000,
+				 (px4_main_t)&test_runner,
+				 nullptr);
 }
 
 int px4muorb_topic_advertised(const char *topic_name)

src/modules/navigator/CMakeLists.txt

@@ -46,6 +46,7 @@ px4_add_module(
 		takeoff.cpp
 		land.cpp
 		precland.cpp
+		precland_mode.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
 		vtol_takeoff.cpp

src/modules/navigator/mission.cpp

@@ -176,10 +176,6 @@ Mission::on_inactivation()
 	_navigator->stop_capturing_images();
 	_navigator->release_gimbal_control();
 
-	if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
-
 	_time_mission_deactivated = hrt_absolute_time();
 
 	/* reset so current mission item gets restarted if mission was paused */
@@ -298,10 +294,25 @@ Mission::on_active()
 	}
 
 	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
-		_navigator->get_precland()->on_active();
+		// Update the position in the setpoint triplet.
+		_precland.setAcceptanceRadius(_navigator->get_acceptance_radius());
+		_precland.update();
+		PrecLand::Output prec_land_output{_precland.getOutput()};
 
-	} else if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
+		_mission_item.altitude = prec_land_output.alt;
+		_mission_item.lat = prec_land_output.pos_hor.lat;
+		_mission_item.lon = prec_land_output.pos_hor.lon;
+		_mission_item.nav_cmd = prec_land_output.nav_cmd;
+
+		mission_apply_limitation(_mission_item);
+
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->next.valid = false;
+
+		if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
+			_navigator->set_position_setpoint_triplet_updated();
+		}
 	}
 }
 
@@ -443,7 +454,9 @@ Mission::find_mission_land_start()
 			_landing_start_lon = missionitem.lon;
 			_landing_start_alt = missionitem.altitude_is_relative ?	missionitem.altitude +
 					     _navigator->get_home_position()->alt : missionitem.altitude;
-			_landing_loiter_radius = missionitem.loiter_radius;
+			_landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius)
+						  && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) :
+						 _navigator->get_loiter_radius();
 			_land_start_available = true;
 		}
 
@@ -981,13 +994,17 @@ Mission::set_mission_items()
 						new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
 
 						if (_mission_item.land_precision == 1) {
-							_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
+							_precland.setMode(PrecLand::PrecLandMode::Opportunistic);
 
 						} else { //_mission_item.land_precision == 2
-							_navigator->get_precland()->set_mode(PrecLandMode::Required);
+							_precland.setMode(PrecLand::PrecLandMode::Required);
 						}
 
-						_navigator->get_precland()->on_activation();
+						PrecLand::LandingPosition2D approximate_landing_pos{
+							.lat = _mission_item.lat,
+							.lon = _mission_item.lon,
+						};
+						_precland.initialize(approximate_landing_pos);
 
 					}
 				}
@@ -1000,13 +1017,17 @@ Mission::set_mission_items()
 						new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
 
 						if (_mission_item.land_precision == 1) {
-							_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
+							_precland.setMode(PrecLand::PrecLandMode::Opportunistic);
 
 						} else { //_mission_item.land_precision == 2
-							_navigator->get_precland()->set_mode(PrecLandMode::Required);
+							_precland.setMode(PrecLand::PrecLandMode::Required);
 						}
 
-						_navigator->get_precland()->on_activation();
+						PrecLand::LandingPosition2D approximate_landing_pos{
+							.lat = _mission_item.lat,
+							.lon = _mission_item.lon,
+						};
+						_precland.initialize(approximate_landing_pos);
 
 					}
 

src/modules/navigator/mission.h

@@ -49,6 +49,7 @@
 #include "mission_block.h"
 #include "mission_feasibility_checker.h"
 #include "navigator_mode.h"
+#include "precland.h"
 
 #include <float.h>
 
@@ -270,6 +271,8 @@ private:
 
 	hrt_abstime _time_mission_deactivated{0};
 
+	PrecLand _precland;
+
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_MISSION

src/modules/navigator/mission_block.cpp

@@ -59,14 +59,7 @@ using matrix::wrap_pi;
 MissionBlock::MissionBlock(Navigator *navigator) :
 	NavigatorMode(navigator)
 {
-	_mission_item.lat = (double)NAN;
-	_mission_item.lon = (double)NAN;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _navigator->get_loiter_radius();
-	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-	_mission_item.time_inside = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
+
 }
 
 bool
@@ -198,6 +191,11 @@ MissionBlock::is_mission_item_reached_or_completed()
 
 		const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item);
 
+		// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
+		const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
+				&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
+				_navigator->get_loiter_radius();
+
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl,
 				_navigator->get_global_position()->lat,
 				_navigator->get_global_position()->lon,
@@ -261,7 +259,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 			 */
 
 			// check if within loiter radius around wp, if yes then set altitude sp to mission item
-			if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
+			if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs)
 			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 				_waypoint_position_reached = true;
@@ -283,7 +281,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 						&dist_xy, &dist_z);
 
 				// check if within loiter radius around wp, if yes then set altitude sp to mission item
-				if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
+				if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs)
 				    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 					curr_sp->alt = mission_item_altitude_amsl;
@@ -291,7 +289,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 					_navigator->set_position_setpoint_triplet_updated();
 				}
 
-			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
+			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs)
 				   && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 				// loitering, check if new altitude is reached, while still also having check on position
 
@@ -516,7 +514,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 				float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
 
 				// calculate (positive) angle between current bearing vector (orbit center to next waypoint) and vector pointing to tangent exit location
-				const float ratio = math::min(fabsf(_mission_item.loiter_radius / range), 1.0f);
+				const float ratio = math::min(fabsf(curr_sp.loiter_radius / range), 1.0f);
 				float inner_angle = acosf(ratio);
 
 				// Compute "ideal" tangent origin
@@ -932,3 +930,16 @@ MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item)
 		return mission_item.altitude;
 	}
 }
+
+void
+MissionBlock::initialize()
+{
+	_mission_item.lat = (double)NAN;
+	_mission_item.lon = (double)NAN;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _navigator->get_loiter_radius();
+	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+	_mission_item.time_inside = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
+}

src/modules/navigator/mission_block.h

@@ -71,6 +71,8 @@ public:
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
+	void initialize() override;
+
 	/**
 	 * Check if the mission item contains a navigation position
 	 *

src/modules/navigator/navigator.h

@@ -43,7 +43,7 @@
 
 #include "geofence.h"
 #include "land.h"
-#include "precland.h"
+#include "precland_mode.h"
 #include "loiter.h"
 #include "mission.h"
 #include "navigator_mode.h"
@@ -172,8 +172,6 @@ public:
 	vehicle_local_position_s    *get_local_position() { return &_local_pos; }
 	vehicle_status_s            *get_vstatus() { return &_vstatus; }
 
-	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
-
 	const vehicle_roi_s &get_vroi() { return _vroi; }
 
 	void reset_vroi() { _vroi = {}; }
@@ -391,7 +389,7 @@ private:
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
 	VtolTakeoff	_vtol_takeoff;			/**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
 	Land		_land;			/**< class for handling land commands */
-	PrecLand	_precland;			/**< class for handling precision land commands */
+	PrecLandMode	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
 
 	NavigatorMode *_navigation_mode{nullptr};	/**< abstract pointer to current navigation mode class */

src/modules/navigator/navigator_main.cpp

@@ -88,6 +88,13 @@ Navigator::Navigator() :
 	_navigation_mode_array[5] = &_precland;
 	_navigation_mode_array[6] = &_vtol_takeoff;
 
+	/* iterate through navigation modes and initialize _mission_item for each */
+	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+		if (_navigation_mode_array[i]) {
+			_navigation_mode_array[i]->initialize();
+		}
+	}
+
 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
 
@@ -337,7 +344,7 @@ void Navigator::run()
 					}
 
 					if (only_alt_change_requested) {
-						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
+						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
 							rep->current.loiter_radius = curr->current.loiter_radius;
 
 
@@ -552,12 +559,6 @@ void Navigator::run()
 				// reset cruise speed and throttle to default when transitioning (VTOL Takeoff handles it separately)
 				reset_cruising_speed();
 				set_cruising_throttle();
-
-				// need to update current setpooint with reset cruise speed and throttle
-				position_setpoint_triplet_s *rep = get_reposition_triplet();
-				*rep = *(get_position_setpoint_triplet());
-				rep->current.cruising_speed = get_cruising_speed();
-				rep->current.cruising_throttle = get_cruising_throttle();
 			}
 		}
 
@@ -705,7 +706,7 @@ void Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_precland;
-			_precland.set_mode(PrecLandMode::Required);
+			_precland.set_mode(PrecLand::PrecLandMode::Required);
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL:

src/modules/navigator/navigator_mode.h

@@ -49,7 +49,8 @@ public:
 	NavigatorMode(Navigator *navigator);
 	virtual ~NavigatorMode() = default;
 	NavigatorMode(const NavigatorMode &) = delete;
-	NavigatorMode operator=(const NavigatorMode &) = delete;
+	NavigatorMode &operator=(const NavigatorMode &) = delete;
+	virtual void initialize() = 0;
 
 	void run(bool active);
 

src/modules/navigator/precland.cpp

@@ -39,31 +39,18 @@
  */
 
 #include "precland.h"
-#include "navigator.h"
 
-#include <string.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <math.h>
-#include <fcntl.h>
+#include <drivers/drv_hrt.h>
 
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_command.h>
+using namespace time_literals;
 
 #define SEC2USEC 1000000.0f
 
-#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
+static constexpr hrt_abstime state_timeout{10_s}; // Maximum time to spend in any state
 
 static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
 
-PrecLand::PrecLand(Navigator *navigator) :
-	MissionBlock(navigator),
-	ModuleParams(navigator)
+PrecLand::PrecLand() : ModuleParams(nullptr)
 {
 	_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
 	_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
@@ -72,31 +59,17 @@ PrecLand::PrecLand(Navigator *navigator) :
 }
 
 void
-PrecLand::on_activation()
+PrecLand::initialize(const LandingPosition2D &approximate_landing_pos)
 {
+	_approximate_landing_pos = approximate_landing_pos;
+	_local_pos_sub.update();
 	_state = PrecLandState::Start;
 	_search_cnt = 0;
 	_last_slewrate_time = 0;
-
-	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
+	_point_reached_time = 0u;
 
 	if (!_map_ref.isInitialized()) {
-		_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
-	}
-
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	pos_sp_triplet->next.valid = false;
-	pos_sp_triplet->previous.valid = false;
-
-	// Check that the current position setpoint is valid, otherwise land at current position
-	if (!pos_sp_triplet->current.valid) {
-		PX4_WARN("Reset");
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->current.timestamp = hrt_absolute_time();
+		_map_ref.initReference(_local_pos_sub.get().ref_lat, _local_pos_sub.get().ref_lon);
 	}
 
 	_sp_pev = matrix::Vector2f(0, 0);
@@ -104,26 +77,27 @@ PrecLand::on_activation()
 	_last_slewrate_time = 0;
 
 	switch_to_state_start();
-
-	_is_activated = true;
 }
 
 void
-PrecLand::on_active()
+PrecLand::update()
 {
-	// get new target measurement
-	_target_pose_updated = _target_pose_sub.update(&_target_pose);
+	// get new input measurement
+	_target_pose_updated = _target_pose_sub.update();
+	_land_detected_sub.update();
+	_global_pos_sub.update();
+	_local_pos_sub.update();
 
 	if (_target_pose_updated) {
 		_target_pose_valid = true;
 	}
 
-	if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) {
+	if ((hrt_elapsed_time(&_target_pose_sub.get().timestamp) / 1e6f) > _param_pld_btout.get()) {
 		_target_pose_valid = false;
 	}
 
 	// stop if we are landed
-	if (_navigator->get_land_detected()->landed) {
+	if (_land_detected_sub.get().landed) {
 		switch_to_state_done();
 	}
 
@@ -153,20 +127,13 @@ PrecLand::on_active()
 		break;
 
 	case PrecLandState::Done:
-		// nothing to do
+		run_state_done();
 		break;
 
 	default:
 		// unknown state
 		break;
 	}
-
-}
-
-void
-PrecLand::on_inactivation()
-{
-	_is_activated = false;
 }
 
 void
@@ -186,6 +153,11 @@ PrecLand::updateParams()
 void
 PrecLand::run_state_start()
 {
+	_output.nav_cmd = NAV_CMD_WAYPOINT;
+	_output.pos_hor.lat = _approximate_landing_pos.lat;
+	_output.pos_hor.lon = _approximate_landing_pos.lon;
+	_output.alt = _global_pos_sub.get().alt;
+
 	// check if target visible and go to horizontal approach
 	if (switch_to_state_horizontal_approach()) {
 		return;
@@ -196,20 +168,19 @@ PrecLand::run_state_start()
 		switch_to_state_fallback();
 	}
 
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	float dist = 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);
+	float dist = get_distance_to_next_waypoint(_approximate_landing_pos.lat, _approximate_landing_pos.lon,
+			_global_pos_sub.get().lat, _global_pos_sub.get().lon);
 
 	// check if we've reached the start point
-	if (dist < _navigator->get_acceptance_radius()) {
+	if (dist < _acceptance_radius) {
 		if (!_point_reached_time) {
 			_point_reached_time = hrt_absolute_time();
 		}
 
 		// if we don't see the target after 1 second, search for it
-		if (_param_pld_srch_tout.get() > 0) {
+		if (_param_pld_srch_tout.get() > FLT_EPSILON) {
 
-			if (hrt_absolute_time() - _point_reached_time > 2000000) {
+			if (hrt_absolute_time() - _point_reached_time > 1_s) {
 				if (!switch_to_state_search()) {
 					switch_to_state_fallback();
 				}
@@ -224,16 +195,17 @@ PrecLand::run_state_start()
 void
 PrecLand::run_state_horizontal_approach()
 {
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	_output.nav_cmd = NAV_CMD_WAYPOINT;
+	_output.pos_hor.lat = _approximate_landing_pos.lat;
+	_output.pos_hor.lon = _approximate_landing_pos.lon;
+	_output.alt = _approach_alt;
 
 	// check if target visible, if not go to start
 	if (!check_state_conditions(PrecLandState::HorizontalApproach)) {
 		PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
 
-		// Stay at current position for searching for the landing target
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+		// Stop at current altitude
+		_output.alt = _global_pos_sub.get().alt;
 
 		if (!switch_to_state_start()) {
 			switch_to_state_fallback();
@@ -247,44 +219,41 @@ PrecLand::run_state_horizontal_approach()
 			_point_reached_time = hrt_absolute_time();
 		}
 
-		if (hrt_absolute_time() - _point_reached_time > 2000000) {
+		if (hrt_absolute_time() - _point_reached_time > 2_s) {
 			// if close enough for descent above target go to descend above target
 			if (switch_to_state_descend_above_target()) {
 
 				return;
 			}
 		}
-
 	}
 
-	float x = _target_pose.x_abs;
-	float y = _target_pose.y_abs;
+	float x = _target_pose_sub.get().x_abs;
+	float y = _target_pose_sub.get().y_abs;
 
 	slewrate(x, y);
 
 	// XXX need to transform to GPS coords because mc_pos_control only looks at that
-	_map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
-
-	pos_sp_triplet->current.alt = _approach_alt;
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-
-	_navigator->set_position_setpoint_triplet_updated();
+	_map_ref.reproject(x, y, _output.pos_hor.lat, _output.pos_hor.lon);
+	_approximate_landing_pos.lat = _output.pos_hor.lat;
+	_approximate_landing_pos.lon = _output.pos_hor.lon;
 }
 
 void
 PrecLand::run_state_descend_above_target()
 {
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	_output.nav_cmd = NAV_CMD_LAND;
+	_output.pos_hor.lat = _approximate_landing_pos.lat;
+	_output.pos_hor.lon = _approximate_landing_pos.lon;
+	_output.alt = _global_pos_sub.get().alt;
 
 	// check if target visible
 	if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
 		if (!switch_to_state_final_approach()) {
 			PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
 
-			// Stay at current position for searching for the target
-			pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-			pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-			pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+			// Stay at current altitude for searching for the target
+			_output.nav_cmd = NAV_CMD_WAYPOINT;
 
 			if (!switch_to_state_start()) {
 				switch_to_state_fallback();
@@ -295,37 +264,44 @@ PrecLand::run_state_descend_above_target()
 	}
 
 	// XXX need to transform to GPS coords because mc_pos_control only looks at that
-	_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
-
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
-
-	_navigator->set_position_setpoint_triplet_updated();
+	_map_ref.reproject(_target_pose_sub.get().x_abs, _target_pose_sub.get().y_abs, _output.pos_hor.lat,
+			   _output.pos_hor.lon);
+	_approximate_landing_pos.lat = _output.pos_hor.lat;
+	_approximate_landing_pos.lon = _output.pos_hor.lon;
 }
 
 void
 PrecLand::run_state_final_approach()
 {
-	// nothing to do, will land
+	_output.nav_cmd = NAV_CMD_LAND;
+	_output.pos_hor.lat = _approximate_landing_pos.lat;
+	_output.pos_hor.lon = _approximate_landing_pos.lon;
+	_output.alt = _global_pos_sub.get().alt;
 }
 
 void
 PrecLand::run_state_search()
 {
+	_output.nav_cmd = NAV_CMD_WAYPOINT;
+	_output.pos_hor.lat = _approximate_landing_pos.lat;
+	_output.pos_hor.lon = _approximate_landing_pos.lon;
+
 	// check if we can see the target
 	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
 		if (!_target_acquired_time) {
-			// target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly
+			// target just became visible. Stop climbing, but give it some margin so we don't stop too abruptly
 			_target_acquired_time = hrt_absolute_time();
-			position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-			float new_alt = _navigator->get_global_position()->alt + 1.0f;
-			pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt;
-			_navigator->set_position_setpoint_triplet_updated();
+			float new_alt = _global_pos_sub.get().alt + 1.0f;
+			_output.alt = new_alt < _output.alt ? new_alt : _output.alt;
 		}
 
+	} else {
+		_target_acquired_time = 0u;
+		_output.alt = _local_pos_sub.get().ref_alt + _param_pld_srch_alt.get();
 	}
 
 	// stay at that height for a second to allow the vehicle to settle
-	if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) {
+	if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1_s) {
 		// try to switch to horizontal approach
 		if (switch_to_state_horizontal_approach()) {
 			return;
@@ -333,7 +309,7 @@ PrecLand::run_state_search()
 	}
 
 	// check if search timed out and go to fallback
-	if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) {
+	if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get() * 1_s) {
 		PX4_WARN("Search timed out");
 
 		switch_to_state_fallback();
@@ -343,16 +319,24 @@ PrecLand::run_state_search()
 void
 PrecLand::run_state_fallback()
 {
-	// nothing to do, will land
+	_output.nav_cmd = NAV_CMD_LAND;
+	_output.pos_hor.lat = _approximate_landing_pos.lat;
+	_output.pos_hor.lon = _approximate_landing_pos.lon;
+}
+
+void
+PrecLand::run_state_done()
+{
+	_output.nav_cmd = NAV_CMD_IDLE;
+	_output.pos_hor.lat = _global_pos_sub.get().lat;
+	_output.pos_hor.lon = _global_pos_sub.get().lon;
+	_output.alt = _global_pos_sub.get().alt;
 }
 
 bool
 PrecLand::switch_to_state_start()
 {
 	if (check_state_conditions(PrecLandState::Start)) {
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-		_navigator->set_position_setpoint_triplet_updated();
 		_search_cnt++;
 
 		_point_reached_time = 0;
@@ -370,7 +354,7 @@ PrecLand::switch_to_state_horizontal_approach()
 {
 	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
 		print_state_switch_message("horizontal approach");
-		_approach_alt = _navigator->get_global_position()->alt;
+		_approach_alt = _global_pos_sub.get().alt;
 
 		_point_reached_time = 0;
 
@@ -412,12 +396,6 @@ bool
 PrecLand::switch_to_state_search()
 {
 	PX4_INFO("Climbing to search altitude.");
-	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
-
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get();
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-	_navigator->set_position_setpoint_triplet_updated();
 
 	_target_acquired_time = 0;
 
@@ -430,12 +408,6 @@ bool
 PrecLand::switch_to_state_fallback()
 {
 	print_state_switch_message("fallback");
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-	pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-	pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
-	_navigator->set_position_setpoint_triplet_updated();
 
 	_state = PrecLandState::Fallback;
 	_state_start_time = hrt_absolute_time();
@@ -457,8 +429,6 @@ void PrecLand::print_state_switch_message(const char *state_name)
 
 bool PrecLand::check_state_conditions(PrecLandState state)
 {
-	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
-
 	switch (state) {
 	case PrecLandState::Start:
 		return _search_cnt <= _param_pld_max_srch.get();
@@ -467,10 +437,10 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 
 		// if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore
 		if (_state == PrecLandState::HorizontalApproach) {
-			if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
-			    && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) {
+			if (fabsf(_target_pose_sub.get().x_abs - _local_pos_sub.get().x) < _param_pld_hacc_rad.get()
+			    && fabsf(_target_pose_sub.get().y_abs - _local_pos_sub.get().y) < _param_pld_hacc_rad.get()) {
 				// we've reached the position where we last saw the target. If we don't see it now, we need to do something
-				return _target_pose_valid && _target_pose.abs_pos_valid;
+				return _target_pose_valid && _target_pose_sub.get().abs_pos_valid;
 
 			} else {
 				// We've seen the target sometime during horizontal approach.
@@ -480,7 +450,7 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 		}
 
 		// If we're trying to switch to this state, the target needs to be visible
-		return _target_pose_updated && _target_pose_valid && _target_pose.abs_pos_valid;
+		return _target_pose_updated && _target_pose_valid && _target_pose_sub.get().abs_pos_valid;
 
 	case PrecLandState::DescendAboveTarget:
 
@@ -488,22 +458,22 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 		if (_state == PrecLandState::DescendAboveTarget) {
 			// if we're close to the ground, we're more critical of target timeouts so we quickly go into descend
 			if (check_state_conditions(PrecLandState::FinalApproach)) {
-				return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s
+				return hrt_absolute_time() - _target_pose_sub.get().timestamp < 500_ms; // 0.5s
 
 			} else {
-				return _target_pose_valid && _target_pose.abs_pos_valid;
+				return _target_pose_valid && _target_pose_sub.get().abs_pos_valid;
 			}
 
 		} else {
 			// if not already in this state, need to be above target to enter it
-			return _target_pose_updated && _target_pose.abs_pos_valid
-			       && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
-			       && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get();
+			return _target_pose_updated && _target_pose_sub.get().abs_pos_valid
+			       && fabsf(_target_pose_sub.get().x_abs - _local_pos_sub.get().x) < _param_pld_hacc_rad.get()
+			       && fabsf(_target_pose_sub.get().y_abs - _local_pos_sub.get().y) < _param_pld_hacc_rad.get();
 		}
 
 	case PrecLandState::FinalApproach:
-		return _target_pose_valid && _target_pose.abs_pos_valid
-		       && (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get();
+		return _target_pose_valid && _target_pose_sub.get().abs_pos_valid
+		       && (_target_pose_sub.get().z_abs - _local_pos_sub.get().z) < _param_pld_fappr_alt.get();
 
 	case PrecLandState::Search:
 		return true;
@@ -537,10 +507,10 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
 		dt = 50000 / SEC2USEC;
 
 		// set a best guess for previous setpoints for smooth transition
-		_sp_pev = _map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
-					   _navigator->get_position_setpoint_triplet()->current.lon);
-		_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
-		_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
+		_sp_pev = _map_ref.project(_output.pos_hor.lat,
+					   _output.pos_hor.lon);
+		_sp_pev_prev(0) = _sp_pev(0) - _local_pos_sub.get().vx * dt;
+		_sp_pev_prev(1) = _sp_pev(1) - _local_pos_sub.get().vy * dt;
 	}
 
 	_last_slewrate_time = now;

src/modules/navigator/precland.h

@@ -43,45 +43,61 @@
 #include <matrix/math.hpp>
 #include <lib/geo/geo.h>
 #include <px4_platform_common/module_params.h>
+#include <uORB/uORB.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/landing_target_pose.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
 
-#include "navigator_mode.h"
-#include "mission_block.h"
+#include "navigator/navigation.h"
 
-enum class PrecLandState {
-	Start, // Starting state
-	HorizontalApproach, // Positioning over landing target while maintaining altitude
-	DescendAboveTarget, // Stay over landing target while descending
-	FinalApproach, // Final landing approach, even without landing target
-	Search, // Search for landing target
-	Fallback, // Fallback landing method
-	Done // Done landing
-};
-
-enum class PrecLandMode {
-	Opportunistic = 1, // only do precision landing if landing target visible at the beginning
-	Required = 2 // try to find landing target if not visible at the beginning
-};
-
-class PrecLand : public MissionBlock, public ModuleParams
+class PrecLand : public ModuleParams
 {
 public:
-	PrecLand(Navigator *navigator);
-	~PrecLand() override = default;
+	enum class PrecLandMode {
+		Opportunistic = 1, // only do precision landing if landing target visible at the beginning
+		Required = 2 // try to find landing target if not visible at the beginning
+	};
 
-	void on_activation() override;
-	void on_active() override;
-	void on_inactivation() override;
+	struct LandingPosition2D {
+		double lat;
+		double lon;
+	};
 
-	void set_mode(PrecLandMode mode) { _mode = mode; };
+	struct Output {
+		LandingPosition2D pos_hor;
+		float alt;
+		enum NAV_CMD nav_cmd;
+	};
 
-	PrecLandMode get_mode() { return _mode; };
+public:
+	PrecLand();
+	~PrecLand() = default;
 
-	bool is_activated() { return _is_activated; };
+	void initialize(const LandingPosition2D &approximate_landing_pos);
+	void update();
+
+	Output getOutput() {return _output;};
+
+	void setMode(PrecLandMode mode) { _mode = mode; };
+
+	void setAcceptanceRadius(float acceptance_radius) {_acceptance_radius = acceptance_radius;};
+
+	PrecLandMode getMode() { return _mode; };
 
 private:
+	enum class PrecLandState {
+		Start, // Starting state
+		HorizontalApproach, // Positioning over landing target while maintaining altitude
+		DescendAboveTarget, // Stay over landing target while descending
+		FinalApproach, // Final landing approach, even without landing target
+		Search, // Search for landing target
+		Fallback, // Fallback landing method
+		Done // Done landing
+	};
 
+private:
 	void updateParams() override;
 
 	// run the control loop for each state
@@ -91,6 +107,7 @@ private:
 	void run_state_final_approach();
 	void run_state_search();
 	void run_state_fallback();
+	void run_state_done();
 
 	// attempt to switch to a different state. Returns true if state change was successful, false otherwise
 	bool switch_to_state_start();
@@ -107,9 +124,14 @@ private:
 	bool check_state_conditions(PrecLandState state);
 	void slewrate(float &sp_x, float &sp_y);
 
-	landing_target_pose_s _target_pose{}; /**< precision landing target position */
+	LandingPosition2D _approximate_landing_pos{}; /**< Global position in WGS84 at which to find the landing target.*/
+	float _acceptance_radius{};
+
+	uORB::SubscriptionData<landing_target_pose_s> _target_pose_sub{ORB_ID(landing_target_pose)};
+	uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
+	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global position subscription */
+	uORB::SubscriptionData<vehicle_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)};	/**< local position subscription */
 
-	uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};
 	bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
 	bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
 
@@ -130,7 +152,7 @@ private:
 
 	PrecLandMode _mode{PrecLandMode::Opportunistic};
 
-	bool _is_activated {false}; /**< indicates if precland is activated */
+	Output _output;
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,

src/modules/navigator/precland_mode.cpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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 precland_mode.cpp
+ *
+ * Helper class to do precision landing with a landing target
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ */
+
+#include "precland_mode.h"
+#include "navigator.h"
+
+#include <uORB/topics/position_setpoint_triplet.h>
+
+PrecLandMode::PrecLandMode(Navigator *navigator) :
+	MissionBlock(navigator)
+{
+
+}
+
+void
+PrecLandMode::on_activation()
+{
+	_global_pos_sub.update();
+	// Get the landing position from current position_setpoint, else use the current vehicle position.
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	PrecLand::LandingPosition2D approximate_landing_pos{ .lat = pos_sp_triplet->current.lat,
+			.lon = pos_sp_triplet->current.lon};
+
+	if (!pos_sp_triplet->current.valid) {
+		PX4_WARN("No valid landing position for precision landing. Using current position");
+		approximate_landing_pos.lat = _global_pos_sub.get().lat;
+		approximate_landing_pos.lon = _global_pos_sub.get().lon;
+	}
+
+	_prec_land.initialize(approximate_landing_pos);
+}
+
+void
+PrecLandMode::on_active()
+{
+	_local_pos_sub.update();
+
+	_prec_land.setAcceptanceRadius(_navigator->get_acceptance_radius());
+	_prec_land.update();
+
+	PrecLand::Output prec_land_output{_prec_land.getOutput()};
+
+	_mission_item.nav_cmd = prec_land_output.nav_cmd;
+	_mission_item.lat = prec_land_output.pos_hor.lat;
+	_mission_item.lon = prec_land_output.pos_hor.lon;
+	_mission_item.altitude = prec_land_output.alt;
+	_mission_item.altitude_is_relative = false;
+	_mission_item.yaw = _local_pos_sub.get().heading;
+
+
+	mission_apply_limitation(_mission_item);
+
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->next.valid = false;
+
+	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+
+}
+
+

src/modules/navigator/precland_mode.h

@@ -0,0 +1,69 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2017 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 preclandMode.h
+ *
+ * Helper class to do precision landing with a landing target
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ */
+
+#pragma once
+
+#include "mission_block.h"
+#include "precland.h"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+class PrecLandMode : public MissionBlock
+{
+public:
+	PrecLandMode(Navigator *navigator);
+	~PrecLandMode() override = default;
+
+	void on_activation() override;
+	void on_active() override;
+
+	void set_mode(PrecLand::PrecLandMode mode) { _prec_land.setMode(mode); };
+
+	PrecLand::PrecLandMode get_mode() { return _prec_land.getMode(); };
+private:
+	PrecLand _prec_land;
+
+	uORB::SubscriptionData<vehicle_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};
+};
+
+

src/modules/navigator/rtl.cpp

@@ -69,9 +69,7 @@ RTL::RTL(Navigator *navigator) :
 
 void RTL::on_inactivation()
 {
-	if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
+
 }
 
 void RTL::on_inactive()
@@ -303,10 +301,8 @@ void RTL::on_active()
 	}
 
 	if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
-		_navigator->get_precland()->on_active();
-
-	} else if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
+		// Need to update the position and type on the current setpoint triplet.
+		set_prec_land_item();
 	}
 
 	// Limit rtl time calculation to 1Hz
@@ -566,13 +562,22 @@ void RTL::set_rtl_item()
 			_mission_item.origin = ORIGIN_ONBOARD;
 			_mission_item.land_precision = _param_rtl_pld_md.get();
 
-			if (_mission_item.land_precision == 1) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-				_navigator->get_precland()->on_activation();
+			if (_mission_item.land_precision > 0u && _mission_item.land_precision <= 2u) {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 
-			} else if (_mission_item.land_precision == 2) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Required);
-				_navigator->get_precland()->on_activation();
+				if (_mission_item.land_precision == 1) {
+					_precland.setMode(PrecLand::PrecLandMode::Opportunistic);
+
+				} else if (_mission_item.land_precision == 2) {
+					_precland.setMode(PrecLand::PrecLandMode::Required);
+				}
+
+				PrecLand::LandingPosition2D approximate_landing_pos{
+					.lat = _mission_item.lat,
+					.lon = _mission_item.lon,
+				};
+
+				_precland.initialize(approximate_landing_pos);
 			}
 
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t");
@@ -605,6 +610,29 @@ void RTL::set_rtl_item()
 	}
 }
 
+void RTL::set_prec_land_item()
+{
+	_precland.setAcceptanceRadius(_navigator->get_acceptance_radius());
+	_precland.update();
+	PrecLand::Output prec_land_output{_precland.getOutput()};
+	_mission_item.altitude = prec_land_output.alt;
+	_mission_item.lat = prec_land_output.pos_hor.lat;
+	_mission_item.lon = prec_land_output.pos_hor.lon;
+	_mission_item.nav_cmd = prec_land_output.nav_cmd;
+
+	// Convert mission item to current position setpoint and make it valid.
+	mission_apply_limitation(_mission_item);
+
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->next.valid = false;
+
+	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}
+
 void RTL::advance_rtl()
 {
 	// determines if the vehicle should loiter above land

src/modules/navigator/rtl.h

@@ -45,6 +45,7 @@
 
 #include "navigator_mode.h"
 #include "mission_block.h"
+#include "precland.h"
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
@@ -118,6 +119,8 @@ private:
 
 	void set_rtl_item();
 
+	void set_prec_land_item();
+
 	void advance_rtl();
 
 	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
@@ -135,6 +138,8 @@ private:
 
 	RTLState _rtl_state{RTL_STATE_NONE};
 
+	PrecLand _precland;
+
 	struct RTLPosition {
 		double lat;
 		double lon;

src/modules/payload_deliverer/gripper.cpp

@@ -62,12 +62,38 @@ void Gripper::init(const GripperConfig &config)
 
 	_timeout_us = config.timeout_us;
 
+	// Reset internal states
+	_state = GripperState::IDLE;
+	_last_command_time = 0;
+	_released_state_cache = false;
+	_grabbed_state_cache = false;
+
 	// We have valid gripper type & sensor configuration
 	_valid = true;
 }
 
+void Gripper::deinit()
+{
+	// Reset Config variables
+	_has_feedback_sensor = false;
+	_timeout_us = 0;
+
+	// Reset internal states
+	_state = GripperState::IDLE;
+	_last_command_time = 0;
+	_released_state_cache = false;
+	_grabbed_state_cache = false;
+
+	// Mark gripper instance as invalid
+	_valid = false;
+}
+
 void Gripper::grab()
 {
+	if (_state == GripperState::GRABBING || _state == GripperState::GRABBED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_GRAB);
 	_state = GripperState::GRABBING;
 	_last_command_time = hrt_absolute_time();
@@ -75,6 +101,10 @@ void Gripper::grab()
 
 void Gripper::release()
 {
+	if (_state == GripperState::RELEASING || _state == GripperState::RELEASED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_RELEASE);
 	_state = GripperState::RELEASING;
 	_last_command_time = hrt_absolute_time();
@@ -89,11 +119,13 @@ void Gripper::update()
 	case GripperState::GRABBING:
 		if (_has_feedback_sensor) {
 			// Handle feedback sensor input, return true for now (not supported)
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 			break;
 		}
 
 		if (command_timed_out) {
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 		}
 

src/modules/payload_deliverer/gripper.h

@@ -78,6 +78,9 @@ public:
 	// Initialize the gripper
 	void init(const GripperConfig &config);
 
+	// De-initialize the gripper
+	void deinit();
+
 	// Actuate the gripper to the grab position
 	void grab();
 
@@ -99,9 +102,17 @@ public:
 	// Returns true if in released position either sensed or timeout based
 	bool released() { return _state == GripperState::RELEASED; }
 
-	// Returns true only once after the state transitioned into released
-	// Used for sending vehicle command ack only when the gripper actually releases
-	// in payload deliverer
+	/**
+	 * @brief Returns true once after gripper succeededs in releasing the payload
+	 *
+	 * Note, that this is a single-read for a one successful release operation. Meaning that
+	 * if you command `release()` and then call this function multiple times in the future,
+	 * it will only return `true` only once, and then it will return `false` after it returns
+	 * `true` once.
+	 *
+	 * This is used to detect a single event of successful releasing of a gripper in `payload_deliverer`
+	 * module, which then would send `vehicle_command_ack` message, indicating the successful release.
+	 */
 	bool released_read_once()
 	{
 		if (_released_state_cache) {
@@ -113,6 +124,20 @@ public:
 		}
 	}
 
+	/**
+	 * @brief Returns true once after gripper succeededs in grabbing the payload
+	 */
+	bool grabbed_read_once()
+	{
+		if (_grabbed_state_cache) {
+			_grabbed_state_cache = false;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
 	// Returns true if Gripper output function is assigned properly
 	bool is_valid() const { return _valid; }
 
@@ -128,7 +153,10 @@ private:
 	// Internal state
 	GripperState _state{GripperState::IDLE};
 	hrt_abstime _last_command_time{0};
-	bool _released_state_cache{false}; // Cached flag that is set once gripper release was successful, gets reset once read
+
+	// Cached flag that is set once gripper release/grab was successful, it must get reset when read.
+	bool _released_state_cache{false};
+	bool _grabbed_state_cache{false};
 
 	uORB::Publication<gripper_s> _gripper_pub{ORB_ID(gripper)};
 };