OFFBOARD mode architecture overhaul (#16739)

- handle SET_POSITION_TARGET_LOCAL_NED and SET_POSITION_TARGET_GLOBAL_INT with ORB_ID(trajectory_setpoint)
 - FlightTaskOffboard not needed at all
 - bypass position_setpoint_triplet entirely (start removing extraneous fields)
 - simplify offboard_control_mode to map to supported control modes

msg/offboard_control_mode.msg

@@ -2,12 +2,8 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-bool ignore_thrust
+bool position
-bool ignore_attitude
+bool velocity
-bool ignore_bodyrate_x
+bool acceleration
-bool ignore_bodyrate_y
+bool attitude
-bool ignore_bodyrate_z
+bool body_rate
-bool ignore_position
-bool ignore_velocity
-bool ignore_acceleration_force
-bool ignore_alt_hold

msg/position_setpoint.msg

@@ -8,8 +8,7 @@ uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
 uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
 uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
-uint8 SETPOINT_TYPE_OFFBOARD=6 	# setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
+uint8 SETPOINT_TYPE_FOLLOW_TARGET=6  # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target
-uint8 SETPOINT_TYPE_FOLLOW_TARGET=7  # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target
 
 uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED
 uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
@@ -43,12 +42,6 @@ int8 landing_gear		# landing gear: see definition of the states in landing_gear.
 float32 loiter_radius		# loiter radius (only for fixed wing), in m
 int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
 
-float32 a_x			# acceleration x setpoint
-float32 a_y			# acceleration y setpoint
-float32 a_z			# acceleration z setpoint
-bool acceleration_valid		# true if acceleration setpoint is valid/should be used
-bool acceleration_is_force	# interprete acceleration as force
-
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)

msg/vehicle_command.msg

@@ -18,7 +18,6 @@ uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84		# Takeoff from ground / hand and trans
 uint16 VEHICLE_CMD_NAV_VTOL_LAND = 85			# Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|
 uint16 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90		# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_NAV_GUIDED_MASTER = 91		# set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|
-uint16 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92		# hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_NAV_DELAY = 93				# Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_NAV_LAST = 95			# NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_CONDITION_DELAY = 112		# Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|

msg/vehicle_control_mode.msg

@@ -8,13 +8,10 @@ bool flag_control_auto_enabled			# true if onboard autopilot should act
 bool flag_control_offboard_enabled		# true if offboard control should be used
 bool flag_control_rates_enabled			# true if rates are stabilized
 bool flag_control_attitude_enabled		# true if attitude stabilization is mixed in
-bool flag_control_yawrate_override_enabled		# true if the yaw rate override is enabled
 bool flag_control_rattitude_enabled		# true if rate/attitude stabilization is enabled
-bool flag_control_force_enabled			# true if force control is mixed in
+bool flag_control_acceleration_enabled		# true if acceleration is controlled
-bool flag_control_acceleration_enabled			# true if acceleration is controlled
 bool flag_control_velocity_enabled		# true if horizontal velocity (implies direction) is controlled
 bool flag_control_position_enabled		# true if position is controlled
 bool flag_control_altitude_enabled		# true if altitude is controlled
 bool flag_control_climb_rate_enabled		# true if climb rate is controlled
 bool flag_control_termination_enabled		# true if flighttermination is enabled
-bool flag_control_fixed_hdg_enabled			# true if using a fixed heading for user control

msg/vehicle_status_flags.msg

@@ -28,7 +28,6 @@ bool circuit_breaker_vtol_fw_arming_check 	# set to true if for VTOLs arming in
 
 bool offboard_control_signal_found_once
 bool offboard_control_signal_lost
-bool offboard_control_set_by_command                # true if the offboard mode was set by a mavlink command and should not be overridden by RC
 
 bool rc_signal_found_once
 bool rc_input_blocked                                # set if RC input should be ignored temporarily

src/modules/commander/Commander.cpp

@@ -857,41 +857,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: {
-			transition_result_t res = TRANSITION_DENIED;
-
-			if (_internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) {
-				_main_state_pre_offboard = _internal_state.main_state;
-			}
-
-			if (cmd.param1 > 0.5f) {
-				res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
-
-				if (res == TRANSITION_DENIED) {
-					print_reject_mode("OFFBOARD");
-					_status_flags.offboard_control_set_by_command = false;
-
-				} else {
-					/* Set flag that offboard was set via command, main state is not overridden by rc */
-					_status_flags.offboard_control_set_by_command = true;
-				}
-
-			} else {
-				/* If the mavlink command is used to enable or disable offboard control:
-				 * switch back to previous mode when disabling */
-				res = main_state_transition(_status, _main_state_pre_offboard, _status_flags, &_internal_state);
-				_status_flags.offboard_control_set_by_command = false;
-			}
-
-			if (res == TRANSITION_DENIED) {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-
-			} else {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
-			}
-		}
-		break;
-
 	case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
 			/* switch to RTL which ends the mission */
 			if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
@@ -1644,7 +1609,7 @@ Commander::run()
 				}
 			}
 
-			_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get());
+			_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1e6f);
 
 			param_init_forced = false;
 		}
@@ -3333,55 +3298,39 @@ Commander::update_control_mode()
 		_vehicle_control_mode.flag_control_termination_enabled = true;
 		break;
 
-	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
-			_vehicle_control_mode.flag_control_offboard_enabled = true;
+		_vehicle_control_mode.flag_control_offboard_enabled = true;
 
-			const offboard_control_mode_s &offboard = _offboard_control_mode_sub.get();
+		if (_offboard_control_mode_sub.get().position) {
+			_vehicle_control_mode.flag_control_position_enabled = true;
+			_vehicle_control_mode.flag_control_velocity_enabled = true;
+			_vehicle_control_mode.flag_control_altitude_enabled = true;
+			_vehicle_control_mode.flag_control_climb_rate_enabled = true;
+			_vehicle_control_mode.flag_control_acceleration_enabled = true;
+			_vehicle_control_mode.flag_control_rates_enabled = true;
+			_vehicle_control_mode.flag_control_attitude_enabled = true;
 
-			if (!offboard.ignore_acceleration_force) {
+		} else if (_offboard_control_mode_sub.get().velocity) {
-				// OFFBOARD acceleration
+			_vehicle_control_mode.flag_control_velocity_enabled = true;
-				_vehicle_control_mode.flag_control_acceleration_enabled = true;
+			_vehicle_control_mode.flag_control_altitude_enabled = true;
-				_vehicle_control_mode.flag_control_attitude_enabled = true;
+			_vehicle_control_mode.flag_control_climb_rate_enabled = true;
-				_vehicle_control_mode.flag_control_rates_enabled = true;
+			_vehicle_control_mode.flag_control_acceleration_enabled = true;
+			_vehicle_control_mode.flag_control_rates_enabled = true;
+			_vehicle_control_mode.flag_control_attitude_enabled = true;
 
-			} else if (!offboard.ignore_position) {
+		} else if (_offboard_control_mode_sub.get().acceleration) {
-				// OFFBOARD position
+			_vehicle_control_mode.flag_control_acceleration_enabled = true;
-				_vehicle_control_mode.flag_control_position_enabled = true;
+			_vehicle_control_mode.flag_control_rates_enabled = true;
-				_vehicle_control_mode.flag_control_velocity_enabled = true;
+			_vehicle_control_mode.flag_control_attitude_enabled = true;
-				_vehicle_control_mode.flag_control_altitude_enabled = true;
-				_vehicle_control_mode.flag_control_climb_rate_enabled = true;
-				_vehicle_control_mode.flag_control_attitude_enabled = true;
-				_vehicle_control_mode.flag_control_rates_enabled = true;
 
-			} else if (!offboard.ignore_velocity) {
+		} else if (_offboard_control_mode_sub.get().attitude) {
-				// OFFBOARD velocity
+			_vehicle_control_mode.flag_control_rates_enabled = true;
-				_vehicle_control_mode.flag_control_velocity_enabled = true;
+			_vehicle_control_mode.flag_control_attitude_enabled = true;
-				_vehicle_control_mode.flag_control_altitude_enabled = true;
-				_vehicle_control_mode.flag_control_climb_rate_enabled = true;
-				_vehicle_control_mode.flag_control_attitude_enabled = true;
-				_vehicle_control_mode.flag_control_rates_enabled = true;
 
-			} else if (!offboard.ignore_attitude) {
+		} else if (_offboard_control_mode_sub.get().body_rate) {
-				// OFFBOARD attitude
+			_vehicle_control_mode.flag_control_rates_enabled = true;
-				_vehicle_control_mode.flag_control_attitude_enabled = true;
-				_vehicle_control_mode.flag_control_rates_enabled = true;
-
-			} else if (!offboard.ignore_bodyrate_x || !offboard.ignore_bodyrate_y || !offboard.ignore_bodyrate_z) {
-				// OFFBOARD rate
-				_vehicle_control_mode.flag_control_rates_enabled = true;
-			}
-
-			// TO-DO: Add support for other modes than yawrate control
-			_vehicle_control_mode.flag_control_yawrate_override_enabled = offboard.ignore_bodyrate_x && offboard.ignore_bodyrate_y
-					&& !offboard.ignore_bodyrate_z && !offboard.ignore_attitude;
-
-			// VTOL transition override
-			if (_status.in_transition_mode) {
-				_vehicle_control_mode.flag_control_acceleration_enabled = false;
-				_vehicle_control_mode.flag_control_velocity_enabled = false;
-				_vehicle_control_mode.flag_control_position_enabled = false;
-			}
 		}
+
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
@@ -3979,30 +3928,41 @@ void Commander::UpdateEstimateValidity()
 void
 Commander::offboard_control_update()
 {
-	const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get();
+	bool offboard_available = false;
 
 	if (_offboard_control_mode_sub.updated()) {
-		const offboard_control_mode_s old = offboard_control_mode;
+		const offboard_control_mode_s old = _offboard_control_mode_sub.get();
 
 		if (_offboard_control_mode_sub.update()) {
-			if (old.ignore_thrust != offboard_control_mode.ignore_thrust ||
+			const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get();
-			    old.ignore_attitude != offboard_control_mode.ignore_attitude ||
+
-			    old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x ||
+			if (old.position != ocm.position ||
-			    old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y ||
+			    old.velocity != ocm.velocity ||
-			    old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z ||
+			    old.acceleration != ocm.acceleration ||
-			    old.ignore_position != offboard_control_mode.ignore_position ||
+			    old.attitude != ocm.attitude ||
-			    old.ignore_velocity != offboard_control_mode.ignore_velocity ||
+			    old.body_rate != ocm.body_rate) {
-			    old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force ||
-			    old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) {
 
 				_status_changed = true;
 			}
+
+			if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate) {
+				offboard_available = true;
+			}
 		}
 	}
 
-	_offboard_available.set_state_and_update(
+	if (_offboard_control_mode_sub.get().position && !_status_flags.condition_local_position_valid) {
-		hrt_elapsed_time(&offboard_control_mode.timestamp) < _param_com_of_loss_t.get() * 1e6f,
+		offboard_available = false;
-		hrt_absolute_time());
+
+	} else if (_offboard_control_mode_sub.get().velocity && !_status_flags.condition_local_velocity_valid) {
+		offboard_available = false;
+
+	} else if (_offboard_control_mode_sub.get().acceleration && !_status_flags.condition_local_velocity_valid) {
+		// OFFBOARD acceleration handled by position controller
+		offboard_available = false;
+	}
+
+	_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
 
 	const bool offboard_lost = !_offboard_available.get_state();
 

src/modules/commander/Commander.hpp

@@ -372,8 +372,6 @@ private:
 	bool		_flight_termination_printed{false};
 	bool		_system_power_usb_connected{false};
 
-	main_state_t	_main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL};
-
 	cpuload_s		_cpuload{};
 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_land_detector{};

src/modules/commander/commander_params.c

@@ -349,7 +349,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @max 60
  * @increment 0.01
  */
-PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.5f);
+PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
 
 /**
  * Set offboard loss failsafe mode

src/modules/commander/state_machine_helper.cpp

@@ -905,7 +905,6 @@ void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *arm
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 			return;
 		}
-
 	}
 
 	// If none of the above worked, try to mitigate
@@ -942,7 +941,13 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *
 		return;
 
 	case offboard_loss_rc_actions_t::MANUAL_POSITION:
-		if (status_flags.condition_global_position_valid) {
+		if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		    && status_flags.condition_local_position_valid) {
+
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+			return;
+
+		} else if (status_flags.condition_global_position_valid) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 			return;
 		}
@@ -978,7 +983,6 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 			return;
 		}
-
 	}
 
 	// If none of the above worked, try to mitigate

src/modules/flight_mode_manager/CMakeLists.txt

@@ -58,7 +58,6 @@ list(APPEND flight_tasks_all
 	ManualPositionSmoothVel
 	AutoLineSmoothVel
 	AutoFollowMe
-	Offboard
 	Failsafe
 	Descend
 	Transition

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -204,31 +204,6 @@ void FlightModeManager::start_flight_task()
 		return;
 	}
 
-	// offboard
-	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
-	    && (_vehicle_control_mode_sub.get().flag_control_altitude_enabled ||
-		_vehicle_control_mode_sub.get().flag_control_position_enabled ||
-		_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled ||
-		_vehicle_control_mode_sub.get().flag_control_velocity_enabled ||
-		_vehicle_control_mode_sub.get().flag_control_acceleration_enabled)) {
-
-		should_disable_task = false;
-		FlightTaskError error = switchTask(FlightTaskIndex::Offboard);
-
-		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Offboard activation failed with error: %s", errorToString(error));
-			}
-
-			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			// we want to be in this mode, reset the failure count
-			_task_failure_count = 0;
-		}
-	}
-
 	// Auto-follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
 		should_disable_task = false;

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -66,7 +66,6 @@ enum class WaypointType : int {
 	takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
 	land = position_setpoint_s::SETPOINT_TYPE_LAND,
 	idle = position_setpoint_s::SETPOINT_TYPE_IDLE,
-	offboard = position_setpoint_s::SETPOINT_TYPE_OFFBOARD, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks
 	follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET,
 };
 

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

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(FlightTaskOffboard
-	FlightTaskOffboard.cpp
-)
-
-target_link_libraries(FlightTaskOffboard PUBLIC FlightTask)
-target_include_directories(FlightTaskOffboard PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp

@@ -1,243 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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 FlightTaskOffboard.cpp
- */
-
-#include "FlightTaskOffboard.hpp"
-#include <mathlib/mathlib.h>
-#include <float.h>
-
-using namespace matrix;
-
-bool FlightTaskOffboard::updateInitialize()
-{
-	bool ret = FlightTask::updateInitialize();
-
-	// require valid position / velocity in xy
-	return ret && PX4_ISFINITE(_position(0))
-	       && PX4_ISFINITE(_position(1))
-	       && PX4_ISFINITE(_velocity(0))
-	       && PX4_ISFINITE(_velocity(1));
-}
-
-bool FlightTaskOffboard::activate(const vehicle_local_position_setpoint_s &last_setpoint)
-{
-	bool ret = FlightTask::activate(last_setpoint);
-	_position_setpoint = _position;
-	_velocity_setpoint.setZero();
-	_position_lock.setAll(NAN);
-	return ret;
-}
-
-bool FlightTaskOffboard::update()
-{
-	bool ret = FlightTask::update();
-
-	// reset setpoint for every loop
-	_resetSetpoints();
-
-	_sub_triplet_setpoint.update();
-
-	if (!_sub_triplet_setpoint.get().current.valid) {
-		_setDefaultConstraints();
-		_position_setpoint = _position;
-		return false;
-	}
-
-	// Yaw / Yaw-speed
-	if (_sub_triplet_setpoint.get().current.yaw_valid) {
-		// yaw control required
-		_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
-
-		if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
-			// yawspeed is used as feedforward
-			_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
-		}
-
-	} else if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
-		// only yawspeed required
-		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
-		// set yaw setpoint to NAN since not used
-		_yaw_setpoint = NAN;
-
-	}
-
-	// Loiter
-	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-		// loiter just means that the vehicle should keep position
-		if (!PX4_ISFINITE(_position_lock(0))) {
-			_position_setpoint = _position_lock = _position;
-
-		} else {
-			_position_setpoint = _position_lock;
-		}
-
-		// don't have to continue
-		return ret;
-
-	} else {
-		_position_lock.setAll(NAN);
-	}
-
-	// Takeoff
-	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-		// just do takeoff to default altitude
-		if (!PX4_ISFINITE(_position_lock(0))) {
-			_position_setpoint = _position_lock = _position;
-			_position_setpoint(2) = _position_lock(2) = _position(2) - _param_mis_takeoff_alt.get();
-
-		} else {
-			_position_setpoint = _position_lock;
-		}
-
-		// don't have to continue
-		return ret;
-
-	} else {
-		_position_lock.setAll(NAN);
-	}
-
-	// Land
-	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-		// land with landing speed, but keep position in xy
-		if (!PX4_ISFINITE(_position_lock(0))) {
-			_position_setpoint = _position_lock = _position;
-			_position_setpoint(2) = _position_lock(2) = NAN;
-			_velocity_setpoint(2) = _param_mpc_land_speed.get();
-
-		} else {
-			_position_setpoint = _position_lock;
-			_velocity_setpoint(2) = _param_mpc_land_speed.get();
-		}
-
-		// don't have to continue
-		return ret;
-
-	} else {
-		_position_lock.setAll(NAN);
-	}
-
-	// IDLE
-	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
-		_velocity_setpoint.setNaN();
-		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-		return ret;
-	}
-
-	// Possible inputs:
-	// 1. position setpoint
-	// 2. position setpoint + velocity setpoint (velocity used as feedforward)
-	// 3. velocity setpoint
-	// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported
-	const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid
-				      && _sub_vehicle_local_position.get().xy_valid;
-	const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid
-				     && _sub_vehicle_local_position.get().z_valid;
-	const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid
-				      && _sub_vehicle_local_position.get().v_xy_valid;
-	const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid
-				     && _sub_vehicle_local_position.get().v_z_valid;
-	const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy;
-	const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z;
-	const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid;
-
-	// if nothing is valid in xy, then exit offboard
-	if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
-		return false;
-	}
-
-	// if nothing is valid in z, then exit offboard
-	if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) {
-		return false;
-	}
-
-	// XY-direction
-	if (feedforward_ctrl_xy) {
-		_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
-		_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
-		_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
-		_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
-
-	} else if (position_ctrl_xy) {
-		_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
-		_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
-
-	} else if (velocity_ctrl_xy) {
-
-		if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
-			// in local frame: don't require any transformation
-			_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
-			_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
-
-		} else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
-			// in body frame: need to transorm first
-			// Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
-			_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf(
-							_yaw) * _sub_triplet_setpoint.get().current.vy;
-			_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf(
-							_yaw) * _sub_triplet_setpoint.get().current.vy;
-
-		} else {
-			// no valid frame
-			return false;
-		}
-	}
-
-	// Z-direction
-	if (feedforward_ctrl_z) {
-		_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
-		_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
-
-	} else if (position_ctrl_z) {
-		_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
-
-	} else if (velocity_ctrl_z) {
-		_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
-	}
-
-	// Acceleration
-	// Note: this is not supported yet and will be mapped to normalized thrust directly.
-	if (_sub_triplet_setpoint.get().current.acceleration_valid) {
-		_acceleration_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
-		_acceleration_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
-		_acceleration_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
-	}
-
-	// use default conditions of upwards position or velocity to take off
-	_constraints.want_takeoff = _checkTakeoff();
-
-	return ret;
-}

src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp

@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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 FlightTaskOffboard.hpp
- */
-
-#pragma once
-
-#include "FlightTask.hpp"
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/position_setpoint.h>
-
-class FlightTaskOffboard : public FlightTask
-{
-public:
-	FlightTaskOffboard() = default;
-
-	virtual ~FlightTaskOffboard() = default;
-	bool update() override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
-	bool updateInitialize() override;
-
-protected:
-	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
-
-private:
-	matrix::Vector3f _position_lock{};
-
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
-					(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt
-				       )
-};

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -151,8 +151,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 				}
 			}
 
-			if (!_vcontrol_mode.flag_control_climb_rate_enabled &&
+			if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
-			    !_vcontrol_mode.flag_control_offboard_enabled) {
 
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
 					// STABILIZED mode generate the attitude setpoint from manual user inputs

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1710,15 +1710,25 @@ FixedwingPositionControl::Run()
 		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
-		//Convert Local setpoints to global setpoints
+		// Convert Local setpoints to global setpoints
 		if (_control_mode.flag_control_offboard_enabled) {
-			if (!globallocalconverter_initialized()) {
+			if (!map_projection_initialized(&_global_local_proj_ref)
-				globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon,
+			    || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
-							  _local_pos.ref_alt, _local_pos.ref_timestamp);
 
-			} else {
+				map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
-				globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z,
+								_local_pos.ref_timestamp);
-							      &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt);
+				_global_local_alt0 = _local_pos.ref_alt;
+			}
+
+			vehicle_local_position_setpoint_s trajectory_setpoint;
+
+			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+				map_projection_reproject(&_global_local_proj_ref,
+							 trajectory_setpoint.x, trajectory_setpoint.y,
+							 &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
+
+				_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
+				_pos_sp_triplet.current.valid = true;
 			}
 		}
 
@@ -1736,8 +1746,7 @@ FixedwingPositionControl::Run()
 							       radians(_param_fw_man_p_max.get()));
 			}
 
-			if (_control_mode.flag_control_offboard_enabled ||
+			if (_control_mode.flag_control_position_enabled ||
-			    _control_mode.flag_control_position_enabled ||
 			    _control_mode.flag_control_velocity_enabled ||
 			    _control_mode.flag_control_acceleration_enabled ||
 			    _control_mode.flag_control_altitude_enabled) {
@@ -1874,7 +1883,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
 	bool in_air_alt_control = (!_landed &&
 				   (_control_mode.flag_control_auto_enabled ||
-				    _control_mode.flag_control_offboard_enabled ||
 				    _control_mode.flag_control_velocity_enabled ||
 				    _control_mode.flag_control_altitude_enabled));
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -85,6 +85,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
@@ -143,6 +144,7 @@ private:
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -168,6 +170,9 @@ private:
 
 	perf_counter_t	_loop_perf;				///< loop performance counter
 
+	map_projection_reference_s _global_local_proj_ref{};
+	float	_global_local_alt0{NAN};
+
 	float	_hold_alt{0.0f};				///< hold altitude for altitude mode
 	float	_takeoff_ground_alt{0.0f};			///< ground altitude at which plane was launched
 	float	_hdg_hold_yaw{0.0f};				///< hold heading for velocity mode

src/modules/mavlink/mavlink_receiver.h

@@ -52,6 +52,7 @@
 #include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <lib/systemlib/mavlink_log.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
@@ -93,7 +94,6 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -128,13 +128,6 @@ public:
 
 	static void *start_helper(void *context);
 
-	/**
-	 * Get the cruising speed in offboard control
-	 *
-	 * @return the desired cruising speed for the current flight mode
-	 */
-	float get_offb_cruising_speed();
-
 	/**
 	 * Set the cruising speed in offboard control
 	 *
@@ -258,6 +251,8 @@ private:
 
 	mavlink_status_t		_status{}; ///< receiver status, used for mavlink_parse_char()
 
+	orb_advert_t _mavlink_log_pub{nullptr};
+
 	// subset of MAV_COMPONENTs we support
 	enum SUPPORTED_COMPONENTS : uint8_t {
 		COMP_ID_ALL,
@@ -369,7 +364,6 @@ private:
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<optical_flow_s>			_flow_pub{ORB_ID(optical_flow)};
-	uORB::Publication<position_setpoint_triplet_s>		_pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
 	uORB::Publication<sensor_gps_s>				_gps_pub{ORB_ID(sensor_gps)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
@@ -378,6 +372,7 @@ private:
 	uORB::Publication<vehicle_global_position_s>		_global_pos_pub{ORB_ID(vehicle_global_position)};
 	uORB::Publication<vehicle_land_detected_s>		_land_detector_pub{ORB_ID(vehicle_land_detected)};
 	uORB::Publication<vehicle_local_position_s>		_local_pos_pub{ORB_ID(vehicle_local_position)};
+	uORB::Publication<vehicle_local_position_setpoint_s>	_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 	uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
@@ -407,9 +402,10 @@ private:
 
 	// ORB subscriptions
 	uORB::Subscription	_actuator_armed_sub{ORB_ID(actuator_armed)};
-	uORB::Subscription	_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription	_home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription	_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription	_vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription 	_actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
 
@@ -432,6 +428,9 @@ private:
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
 	uint16_t			_mom_switch_state{0};
 
+	map_projection_reference_s	_global_local_proj_ref{};
+	float				_global_local_alt0{NAN};
+
 	uint64_t			_global_ref_timestamp{0};
 
 	map_projection_reference_s	_hil_local_proj_ref{};
@@ -440,9 +439,6 @@ private:
 
 	hrt_abstime			_last_utm_global_pos_com{0};
 
-	float 				_offb_cruising_speed_mc{-1.0f};
-	float 				_offb_cruising_speed_fw{-1.0f};
-
 	// Allocated if needed.
 	TunePublisher *_tune_publisher{nullptr};
 

src/modules/mc_att_control/mc_att_control.hpp

@@ -103,7 +103,6 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
-	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};		/**< vehicle rates setpoint subscription */
 	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -325,16 +325,6 @@ MulticopterAttitudeControl::Run()
 
 			Vector3f rates_sp = _attitude_control.update(q);
 
-			if (_v_control_mode.flag_control_yawrate_override_enabled) {
-				/* Yaw rate override enabled, overwrite the yaw setpoint */
-				vehicle_rates_setpoint_s v_rates_sp{};
-
-				if (_v_rates_sp_sub.copy(&v_rates_sp)) {
-					const float yawrate_sp = v_rates_sp.yaw;
-					rates_sp(2) = yawrate_sp;
-				}
-			}
-
 			// publish rate setpoint
 			vehicle_rates_setpoint_s v_rates_sp{};
 			v_rates_sp.roll = rates_sp(0);

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -330,6 +330,15 @@ void MulticopterPositionControl::Run()
 				_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
 			}
 
+			if (_control_mode.flag_control_offboard_enabled) {
+				_vehicle_constraints.want_takeoff = _control_mode.flag_armed && hrt_elapsed_time(&_setpoint.timestamp) < 1_s;
+
+				// override with defaults
+				_vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get();
+				_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
+				_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+			}
+
 			// handle smooth takeoff
 			_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff,
 						    _vehicle_constraints.speed_up, false, time_stamp_now);

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -212,9 +212,9 @@ MulticopterRateControl::Run()
 			vehicle_rates_setpoint_s v_rates_sp;
 
 			if (_v_rates_sp_sub.update(&v_rates_sp)) {
-				_rates_sp(0) = v_rates_sp.roll;
+				_rates_sp(0) = PX4_ISFINITE(v_rates_sp.roll)  ? v_rates_sp.roll  : rates(0);
-				_rates_sp(1) = v_rates_sp.pitch;
+				_rates_sp(1) = PX4_ISFINITE(v_rates_sp.pitch) ? v_rates_sp.pitch : rates(1);
-				_rates_sp(2) = v_rates_sp.yaw;
+				_rates_sp(2) = PX4_ISFINITE(v_rates_sp.yaw)   ? v_rates_sp.yaw   : rates(2);
 				_thrust_sp = -v_rates_sp.thrust_body[2];
 			}
 		}

src/modules/navigator/mission.cpp

@@ -1857,11 +1857,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
 		(p1->loiter_direction == p2->loiter_direction) &&
-		(fabsf(p1->a_x - p2->a_x) < FLT_EPSILON) &&
-		(fabsf(p1->a_y - p2->a_y) < FLT_EPSILON) &&
-		(fabsf(p1->a_z - p2->a_z) < FLT_EPSILON) &&
-		(p1->acceleration_valid == p2->acceleration_valid) &&
-		(p1->acceleration_is_force == p2->acceleration_is_force) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
 		(fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) &&
 		((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle)