Log camera topics

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>

CMakeLists.txt

@@ -256,7 +256,7 @@ endif()
 
 set(package-contact "px4users@googlegroups.com")
 
-set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)

ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal

@@ -25,7 +25,6 @@ param set-default FW_W_EN 1
 
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric

@@ -25,7 +25,6 @@ param set-default FW_W_EN 1
 
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo

@@ -25,7 +25,6 @@ param set-default FW_W_EN 1
 
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal

@@ -27,7 +27,6 @@ param set-default FW_W_EN 1
 param set-default MIS_LTRMIN_ALT 30
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -277,7 +277,7 @@ then
 	# Override namespace if environment variable is defined
 	microdds_ns="-n $PX4_MICRODDS_NS"
 fi
-microdds_client start -t udp -p 8888 $microdds_ns
+microdds_client start -t udp -h 127.0.0.1 -p 8888 $microdds_ns
 
 if param greater -s MNT_MODE_IN -1
 then

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -79,7 +79,6 @@ param set-default MC_YAWRATE_MAX 20
 param set-default MC_AIRMODE 1
 
 param set-default MIS_DIST_1WP 100
-param set-default MIS_DIST_WPS 100000
 param set-default MIS_TAKEOFF_ALT 15
 
 param set-default MPC_XY_P 0.8

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -46,7 +46,6 @@ param set-default RTL_LAND_DELAY -1
 #
 param set-default NAV_ACC_RAD 10
 
-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TAKEOFF_ALT 25
 param set-default MIS_TKO_LAND_REQ 2
 

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -23,7 +23,6 @@ param set-default EKF2_FUSE_BETA 1
 
 param set-default HTE_VXY_THR 2.0
 
-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TKO_LAND_REQ 2
 
 param set-default MPC_ACC_HOR_MAX 2

Tools/convert_ip.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+"""
+Converts IP address given in decimal dot notation to int32 to be used in XRCE_DDS_0_CFG parameter
+and vice-versa
+
+@author: Beniamino Pozzan (beniamino.pozzan@phd.unipd.it)
+"""
+
+
+import argparse
+
+parser = argparse.ArgumentParser(
+    prog = 'convert_ip',
+    description = 'converts IP to int32 and viceversa'
+    )
+parser.add_argument('input',
+                    type=str,
+                    help='IP address to convert')
+parser.add_argument('-r','--reverse',
+                    action='store_true',
+                    help='converts from int32 to dot decimal')
+
+args = parser.parse_args()
+
+if( args.reverse == False ):
+
+    try:
+        ip_parts = [int(x) for x in args.input.split('.')]
+    except:
+        raise ValueError("Not a valid IP")
+    if( len(ip_parts)!=4 ):
+        raise ValueError("Not a valid IP")
+    if( not all(x>=0 and x<255 for x in ip_parts) ):
+        raise ValueError("Not a valid IP")
+
+    ip = (ip_parts[0]<<24) + (ip_parts[1]<<16) + (ip_parts[2]<<8) + ip_parts[3]
+
+    if(ip & 0x80000000):
+        ip = -0x100000000 + ip
+
+    print(ip)
+
+else:
+    try:
+        ip = int(args.input)
+    except:
+        raise ValueError("Not a valid IP")
+    if(ip < 0):
+        ip = ip + 0x100000000
+    print('{}.{}.{}.{}'.format(ip>>24, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff))

platforms/common/uORB/Subscription.hpp

@@ -245,71 +245,4 @@ private:
 	T _data{};
 };
 
-template<auto member>
-class SubscriptionSelection;
-
-template <class T, class R, R T::*member>
-class SubscriptionSelection<member>: public Subscription
-{
-public:
-	/**
-	 * Constructor
-	 *
-	 * @param id The uORB metadata ORB_ID enum for the topic.
-	 * @param instance The instance for multi sub.
-	 */
-	SubscriptionSelection(ORB_ID id, uint8_t instance = 0) :
-		Subscription(id, instance)
-	{
-		_copySelection();
-	}
-
-	/**
-	 * Constructor
-	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
-	 * @param instance The instance for multi sub.
-	 */
-	SubscriptionSelection(const orb_metadata *meta, uint8_t instance = 0) :
-		Subscription(meta, instance)
-	{
-		_copySelection();
-	}
-
-	~SubscriptionSelection() = default;
-
-	// no copy, assignment, move, move assignment
-	SubscriptionSelection(const SubscriptionSelection &) = delete;
-	SubscriptionSelection &operator=(const SubscriptionSelection &) = delete;
-	SubscriptionSelection(SubscriptionSelection &&) = delete;
-	SubscriptionSelection &operator=(SubscriptionSelection &&) = delete;
-
-	// update the embedded struct.
-	bool update()
-	{
-		bool updated = Subscription::updated();
-
-		if (updated) {
-			_copySelection();
-		}
-
-		return updated;
-	}
-
-	const R &get() const { return _data; }
-
-private:
-	void _copySelection()
-	{
-		T full_data;
-
-		if (copy(&full_data)) {
-			_data = full_data.*member;
-		}
-	}
-
-	R _data{};
-};
-
-
 } // namespace uORB

src/drivers/rc_input/RCInput.cpp

@@ -325,7 +325,13 @@ void RCInput::Run()
 			updateParams();
 		}
 
-		_vehicle_status_arming_state_sub.update();
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status;
+
+			if (_vehicle_status_sub.copy(&vehicle_status)) {
+				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			}
+		}
 
 		const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
@@ -340,7 +346,7 @@ void RCInput::Run()
 				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #if defined(SPEKTRUM_POWER)
 
-				if (!_rc_scan_locked && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
+				if (!_rc_scan_locked && !_armed) {
 					if ((int)vcmd.param1 == 0) {
 						// DSM binding command
 						int dsm_bind_mode = (int)vcmd.param2;
@@ -752,8 +758,7 @@ void RCInput::Run()
 
 			_to_input_rc.publish(_rc_in);
 
-		} else if (!rc_updated && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)
-			   && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -762,7 +767,7 @@ void RCInput::Run()
 		}
 
 		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && rc_updated && _rc_scan_locked
+		if (!_armed && rc_updated && _rc_scan_locked
 		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
 		    && (_param_rc_input_proto.get() < 0)
 		   ) {

src/drivers/rc_input/RCInput.hpp

@@ -138,13 +138,16 @@ private:
 
 	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
-	uORB::SubscriptionSelection<&vehicle_status_s::arming_state>	_vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	input_rc_s	_rc_in{};
 
 	float		_analog_rc_rssi_volt{-1.0f};
 	bool		_analog_rc_rssi_stable{false};
 
+	bool _armed{false};
+
+
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};

src/modules/camera_feedback/CameraFeedback.cpp

@@ -70,27 +70,28 @@ CameraFeedback::Run()
 
 	camera_trigger_s trig{};
 
-	while (_trigger_sub.update(&trig)) {
+	vehicle_attitude_s att{};
 
+	while (_att_sub.update(&att)) {
 		// update geotagging subscriptions
 		vehicle_global_position_s gpos{};
 		_gpos_sub.copy(&gpos);
 
-		vehicle_attitude_s att{};
 		_att_sub.copy(&att);
+		_trigger_sub.copy(&trig);
 
-		if (trig.timestamp == 0 ||
-		    gpos.timestamp == 0 ||
-		    att.timestamp == 0) {
+		// if (trig.timestamp == 0 ||
+		//     gpos.timestamp == 0 ||
+		//     att.timestamp == 0) {
 
-			// reject until we have valid data
-			continue;
-		}
+		// 	// reject until we have valid data
+		// 	continue;
+		// }
 
-		if ((_cam_cap_fback >= 1) && !trig.feedback) {
-			// Ignore triggers that are not feedback when camera capture feedback is enabled
-			continue;
-		}
+		// if ((_cam_cap_fback >= 1) && !trig.feedback) {
+		// 	// Ignore triggers that are not feedback when camera capture feedback is enabled
+		// 	continue;
+		// }
 
 		camera_capture_s capture{};
 

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -106,8 +106,14 @@ void FwAutotuneAttitudeControl::Run()
 		return;
 	}
 
-	_vehicle_status_armed_state_sub.update();
-	_vehicle_status_nav_state_sub.update();
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_nav_state = vehicle_status.nav_state;
+		}
+	}
 
 	if (_actuator_controls_status_sub.updated()) {
 		actuator_controls_status_s controls_status;
@@ -290,7 +296,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			mavlink_log_info(&_mavlink_log_pub, "Autotune started");
 			_state = state::init;
 			_state_start_time = now;
-			_start_flight_mode = _vehicle_status_nav_state_sub.get();
+			_start_flight_mode = _nav_state;
 		}
 
 		break;
@@ -423,7 +429,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::wait_for_disarm:
-		if (!(_vehicle_status_armed_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
+		if (!_armed) {
 			saveGainsToParams();
 			_state = state::complete;
 			_state_start_time = now;
@@ -475,7 +481,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	if (_state != state::wait_for_disarm && _state != state::idle && _state != state::fail && _state != state::complete) {
 		if (now - _state_start_time > 20_s
 		    || (_param_fw_at_man_aux.get() && !_aux_switch_en)
-		    || _start_flight_mode != _vehicle_status_nav_state_sub.get()) {
+		    || _start_flight_mode != _nav_state) {
 			orb_advert_t mavlink_log_pub = nullptr;
 			mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing");
 			_state = state::fail;

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp

@@ -114,8 +114,7 @@ private:
 	uORB::Subscription _actuator_controls_status_sub;
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_armed_state_sub{ORB_ID(vehicle_status)};
-	uORB::SubscriptionSelection<&vehicle_status_s::nav_state> _vehicle_status_nav_state_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -143,6 +142,8 @@ private:
 	uint8_t _max_steps{5};
 	int8_t _signal_sign{0};
 
+	bool _armed{false};
+	uint8_t _nav_state{0};
 	uint8_t _start_flight_mode{0};
 	bool _aux_switch_en{false};
 

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -709,8 +709,24 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 	_skipping_takeoff_detection = false;
 
-	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
-	     _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) {
+	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid) {
+		if (_control_mode.flag_control_position_enabled) {
+			if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
+			    && PX4_ISFINITE(_pos_sp_triplet.current.vz)) {
+				// Offboard position with velocity setpoints
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_PATH;
+				return;
+
+			} else {
+				// Offboard position setpoint only
+				_control_mode_current = FW_POSCTRL_MODE_AUTO;
+				return;
+			}
+		}
+	}
+
+	if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
+	    && _position_setpoint_current_valid) {
 
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
@@ -1097,17 +1113,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 
-	if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
-		// Navigate directly on position setpoint and path tangent
-		matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
-		float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
-				  0.0f;
-		navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed,
-				    _wind_vel, curvature);
-
-	} else {
-		navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
-	}
+	navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 
 	_att_sp.roll_body = _npfg.getRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1282,6 +1288,57 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   _param_climbrate_target.get());
 }
 
+void
+FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos,
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
+{
+
+	float tecs_fw_thr_min;
+	float tecs_fw_thr_max;
+
+	if (pos_sp_curr.gliding_enabled) {
+		/* enable gliding with this waypoint */
+		_tecs.set_speed_weight(2.0f);
+		tecs_fw_thr_min = 0.0;
+		tecs_fw_thr_max = 0.0;
+
+	} else {
+		tecs_fw_thr_min = _param_fw_thr_min.get();
+		tecs_fw_thr_max = _param_fw_thr_max.get();
+	}
+
+	// waypoint is a plain navigation waypoint
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
+				_param_fw_airspd_min.get(), ground_speed);
+
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+
+	// Navigate directly on position setpoint and path tangent
+	matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
+	float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
+			  0.0f;
+	navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);
+
+	_att_sp.roll_body = _npfg.getRollSetpoint();
+	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+	tecs_update_pitch_throttle(control_interval,
+				   pos_sp_curr.alt,
+				   target_airspeed,
+				   radians(_param_fw_p_lim_min.get()),
+				   radians(_param_fw_p_lim_max.get()),
+				   tecs_fw_thr_min,
+				   tecs_fw_thr_max,
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get());
+}
+
 void
 FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
 		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
@@ -2345,6 +2402,11 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
+		case FW_POSCTRL_MODE_AUTO_PATH: {
+				control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
+				break;
+			}
+
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
 				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
 				break;

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -243,6 +243,7 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
+		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
@@ -585,6 +586,18 @@ private:
 	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
 
+	/**
+	 * @brief Vehicle control for following a path.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+			       const position_setpoint_s &pos_sp_curr);
+
 	/**
 	 * @brief Controls a desired airspeed, bearing, and height rate.
 	 *

src/modules/logger/logged_topics.cpp

@@ -51,7 +51,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
 	add_optional_topic("autotune_attitude_control_status", 100);
-	add_optional_topic("camera_capture");
+	add_topic("camera_capture");
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -96,7 +96,13 @@ void McAutotuneAttitudeControl::Run()
 		return;
 	}
 
-	_vehicle_status_arming_state_sub.update();
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
 
 	if (_actuator_controls_status_sub.updated()) {
 		actuator_controls_status_s controls_status;
@@ -393,7 +399,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::wait_for_disarm:
-		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
+		if (!_armed) {
 			saveGainsToParams();
 			_state = state::complete;
 			_state_start_time = now;

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -108,7 +108,7 @@ private:
 	uORB::Subscription _actuator_controls_status_sub{ORB_ID(actuator_controls_status_0)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -136,6 +136,8 @@ private:
 	uint8_t _max_steps{5};
 	int8_t _signal_sign{0};
 
+	bool _armed{false};
+
 	matrix::Vector3f _kid{};
 	matrix::Vector3f _rate_k{};
 	matrix::Vector3f _rate_i{};

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -151,13 +151,18 @@ void MulticopterHoverThrustEstimator::Run()
 
 	perf_begin(_cycle_perf);
 
-	_vehicle_status_arming_state_sub.update();
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
 
 	const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f;
 	_timestamp_last = local_pos.timestamp;
 
-	if ((_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && _in_air && (dt > 0.001f)
-	    && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
+	if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
 
 		_hover_thrust_ekf.predict(dt);
 
@@ -193,7 +198,7 @@ void MulticopterHoverThrustEstimator::Run()
 	} else {
 		_valid_hysteresis.set_state_and_update(false, hrt_absolute_time());
 
-		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
+		if (!_armed) {
 			reset();
 		}
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -102,11 +102,12 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
 	hrt_abstime _timestamp_last{0};
 
+	bool _armed{false};
 	bool _landed{false};
 	bool _in_air{false};
 

src/modules/microdds_client/microdds_client.cpp

@@ -79,7 +79,7 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta
 	}
 }
 
-MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host,
+MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip,
 			       const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) :
 	ModuleParams(nullptr),
 	_localhost_only(localhost_only), _custom_participant(custom_participant),
@@ -122,9 +122,11 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud
 
 #if defined(MICRODDS_CLIENT_UDP)
 		_transport_udp = new uxrUDPTransport();
+		strncpy(_port, port, PORT_MAX_LENGTH - 1);
+		strncpy(_agent_ip, agent_ip, AGENT_IP_MAX_LENGTH - 1);
 
 		if (_transport_udp) {
-			if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, host, port)) {
+			if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) {
 				_comm = &_transport_udp->comm;
 				_fd = _transport_udp->platform.poll_fd.fd;
 
@@ -265,7 +267,7 @@ void MicroddsClient::run()
 				   "</dds>"
 				  );
 
-		if (ret < 0 || ret >= TOPIC_NAME_SIZE) {
+		if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) {
 			PX4_ERR("create entities failed: namespace too long");
 			return;
 		}
@@ -546,8 +548,26 @@ int MicroddsClient::task_spawn(int argc, char *argv[])
 int MicroddsClient::print_status()
 {
 	PX4_INFO("Running, %s", _connected ? "connected" : "disconnected");
-	PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
-	PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
+#if defined(MICRODDS_CLIENT_UDP)
+
+	if (_transport_udp != nullptr) {
+		PX4_INFO("Using transport: udp");
+		PX4_INFO("Agent IP: %s", _agent_ip);
+		PX4_INFO("Agent port: %s", _port);
+
+	}
+
+#endif
+
+	if (_transport_serial != nullptr) {
+		PX4_INFO("Using transport: serial");
+	}
+
+	if (_connected) {
+		PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
+		PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
+	}
+
 	return 0;
 }
 
@@ -558,19 +578,19 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = nullptr;
 
+	char port[PORT_MAX_LENGTH] = {0};
+	char agent_ip[AGENT_IP_MAX_LENGTH] = {0};
+
 #if defined(MICRODDS_CLIENT_UDP)
 	Transport transport = Transport::Udp;
-
 #else
 	Transport transport = Transport::Serial;
 #endif
 	const char *device = nullptr;
 	int baudrate = 921600;
 
-	const char *port = "8888";
 	bool localhost_only = false;
 	bool custom_participant = false;
-	const char *ip = "127.0.0.1";
 
 	const char *client_namespace = nullptr;//"px4";
 
@@ -605,11 +625,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 #if defined(MICRODDS_CLIENT_UDP)
 
 		case 'h':
-			ip = myoptarg;
+			snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%s", myoptarg);
 			break;
 
 		case 'p':
-			port = myoptarg;
+			snprintf(port, PORT_MAX_LENGTH, "%s", myoptarg);
 			break;
 
 		case 'l':
@@ -636,6 +656,33 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 		}
 	}
 
+#if defined(MICRODDS_CLIENT_UDP)
+
+	if (port[0] == '\0') {
+		// no port specified, use XRCE_DDS_PRT
+		int32_t port_i = 0;
+		param_get(param_find("XRCE_DDS_PRT"), &port_i);
+
+		if (port_i < 0 || port_i > 65535) {
+			PX4_ERR("port must be between 0 and 65535");
+			return nullptr;
+		}
+
+		snprintf(port, PORT_MAX_LENGTH, "%u", (uint16_t)port_i);
+	}
+
+	if (agent_ip[0] == '\0') {
+		// no agent ip specified, use XRCE_DDS_AG_IP
+		int32_t ip_i = 0;
+		param_get(param_find("XRCE_DDS_AG_IP"), &ip_i);
+		snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%u.%u.%u.%u", static_cast<uint8_t>(((ip_i) >> 24) & 0xff),
+			 static_cast<uint8_t>(((ip_i) >> 16) & 0xff),
+			 static_cast<uint8_t>(((ip_i) >> 8) & 0xff),
+			 static_cast<uint8_t>(ip_i & 0xff));
+	}
+
+#endif // MICRODDS_CLIENT_UDP
+
 	if (error_flag) {
 		return nullptr;
 	}
@@ -647,7 +694,8 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 		}
 	}
 
-	return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, custom_participant, client_namespace);
+	return new MicroddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant,
+				  client_namespace);
 }
 
 int MicroddsClient::print_usage(const char *reason)
@@ -671,8 +719,8 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555
 	PRINT_MODULE_USAGE_PARAM_STRING('t', "udp", "serial|udp", "Transport protocol", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "serial device", true);
 	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:<param_name>)", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "<IP>", "Host IP", true);
-	PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('h', nullptr, "<IP>", "Agent IP. If not provided, defaults to XRCE_DDS_AG_IP", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 65535, "Agent listening port. If not provided, defaults to XRCE_DDS_PRT", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('c', "Use custom participant config (profile_name=\"px4_participant\")", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true);

src/modules/microdds_client/microdds_client.h

@@ -78,6 +78,17 @@ private:
 	const bool _custom_participant;
 	const char *_client_namespace;
 
+
+	// max port characters (5+'\0')
+	static const uint8_t PORT_MAX_LENGTH = 6;
+	// max agent ip characters (15+'\0')
+	static const uint8_t AGENT_IP_MAX_LENGTH = 16;
+
+#if defined(CONFIG_NET) || defined(__PX4_POSIX)
+	char _port[PORT_MAX_LENGTH];
+	char _agent_ip[AGENT_IP_MAX_LENGTH];
+#endif
+
 	SendTopicsSubs *_subs{nullptr};
 	RcvTopicsPubs *_pubs{nullptr};
 

src/modules/microdds_client/module.yaml

@@ -1,16 +1,3 @@
-
-
-# parameters to auto start
-#  mode (normal, minimal)
-#  UDP port
-#  max rate
-#  DDS DOMAIN ID
-#
-
-
-# multiple instances?
-
-
 module_name: Micro XRCE-DDS
 serial_config:
     - command: |
@@ -22,13 +9,8 @@ serial_config:
         microdds_client start ${XRCE_DDS_ARGS}
 
       port_config_param:
-        name: XRCE_DDS_${i}_CFG
+        name: XRCE_DDS_CFG
         group: Micro XRCE-DDS
-        # MAVLink instances:
-        # 0: Telem1 Port (Telemetry Link)
-        # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage
-        # 2: Board-specific / no fixed function or port
-        #default: [TEL1, "", ""]
       supports_networking: true
 
 parameters:
@@ -46,7 +28,7 @@ parameters:
 
         XRCE_DDS_KEY:
             description:
-                short: XRCE DDS key
+                short: XRCE DDS Session key
                 long: |
                     XRCE DDS key, must be different from zero.
                     In a single agent - multi client configuration, each client
@@ -56,13 +38,29 @@ parameters:
             reboot_required: true
             default: 1
 
-        XRCE_DDS_UDP_PRT:
+        XRCE_DDS_PRT:
             description:
                 short: Micro DDS UDP Port
                 long: |
                     If ethernet enabled and selected as configuration for micro DDS,
                     selected udp port will be set and used.
             type: int32
+            min: 0
+            max: 65535
             reboot_required: true
             default: 8888
             requires_ethernet: true
+
+        XRCE_DDS_AG_IP:
+            description:
+                short: Micro DDS Agent IP address
+                long: |
+                    If ethernet enabled and selected as configuration for micro DDS,
+                    selected Agent IP address will be set and used.
+                    Decimal dot notation is not supported. IP address must be provided
+                    in int32 format. For example, 192.168.1.2 is mapped to -1062731518;
+                    127.0.0.1 is mapped to 2130706433.
+            type: int32
+            reboot_required: true
+            default: 2130706433
+            requires_ethernet: true

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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
@@ -50,6 +50,7 @@ void FeasibilityChecker::reset()
 	_is_landed = false;
 	_home_alt_msl = NAN;
 	_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
+	_current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	_vehicle_type = VehicleType::RotaryWing;
 
 	_mission_validity_failed = false;
@@ -119,6 +120,12 @@ void FeasibilityChecker::updateData()
 		_is_landed = land_detected.landed;
 	}
 
+	if (_vehicle_global_position_sub.updated()) {
+		vehicle_global_position_s vehicle_global_position = {};
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
+		_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
+	}
+
 	param_t handle = param_find("FW_LND_ANG");
 
 	if (handle != PARAM_INVALID) {
@@ -131,12 +138,6 @@ void FeasibilityChecker::updateData()
 		param_get(handle, &_param_mis_dist_1wp);
 	}
 
-	handle = param_find("MIS_DIST_WPS");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_param_mis_dist_wps);
-	}
-
 	handle = param_find("NAV_ACC_RAD");
 
 	if (handle != PARAM_INVALID) {
@@ -195,7 +196,7 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
 	}
 
 	if (!_distance_first_waypoint_failed) {
-		_distance_first_waypoint_failed = !checkDistanceToFirstWaypoint(mission_item);
+		_distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item);
 	}
 
 	if (!_below_home_alt_failed) {
@@ -601,50 +602,44 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 }
 
 
-bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item)
+bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
 {
-	if (_param_mis_dist_1wp <= 0.0f || !_home_lat_lon.isAllFinite()) {
-		/* param not set, check is ok */
-		return true;
-	}
+	if (_param_mis_dist_1wp > FLT_EPSILON &&
+	    (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found &&
+	    MissionBlock::item_contains_position(mission_item)) {
 
-	if (!_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) {
 		_first_waypoint_found = true;
 
+		float dist_to_1wp_from_current_pos = 1e6f;
 
-		/* check distance from current position to item */
-		float dist_to_1wp = get_distance_to_next_waypoint(
-					    mission_item.lat, mission_item.lon,
-					    _home_lat_lon(0), _home_lat_lon(1));
+		if (_current_position_lat_lon.isAllFinite()) {
+			dist_to_1wp_from_current_pos = get_distance_to_next_waypoint(
+							       mission_item.lat, mission_item.lon,
+							       _current_position_lat_lon(0), _current_position_lat_lon(1));
+		}
 
-		if (dist_to_1wp < _param_mis_dist_1wp) {
+		if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) {
 
 			return true;
 
 		} else {
-			/* item is too far from home */
+			/* item is too far from current position */
 			mavlink_log_critical(_mavlink_log_pub,
 					     "First waypoint too far away: %dm, %d max\t",
-					     (int)dist_to_1wp, (int)_param_mis_dist_1wp);
+					     (int)dist_to_1wp_from_current_pos, (int)_param_mis_dist_1wp);
 			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info},
-							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)_param_mis_dist_1wp);
+							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp_from_current_pos,
+							 (uint32_t)_param_mis_dist_1wp);
 
 			return false;
 		}
 	}
 
-	/* no waypoints found in mission, then we will not fly far away */
 	return true;
 }
 
 bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mission_item)
 {
-	if (_param_mis_dist_wps <= 0.0f) {
-		/* param not set, check is ok */
-		return true;
-	}
-
-
 	/* check only items with valid lat/lon */
 	if (!MissionBlock::item_contains_position(mission_item)) {
 		return true;
@@ -658,24 +653,8 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi
 				_last_lat, _last_lon);
 
 
-		if (dist_between_waypoints > _param_mis_dist_wps) {
-			/* distance between waypoints is too high */
-			mavlink_log_critical(_mavlink_log_pub,
-					     "Distance between waypoints too far: %d meters, %d max.\t",
-					     (int)dist_between_waypoints, (int)_param_mis_dist_wps);
-			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info},
-							 "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints,
-							 (uint32_t)_param_mis_dist_wps);
-
-
-			return false;
-
-			/* do not allow waypoints that are literally on top of each other */
-
-			/* and do not allow condition gates that are at the same position as a navigation waypoint */
-
-		} else if (dist_between_waypoints < 0.05f &&
-			   (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) {
+		if (dist_between_waypoints < 0.05f &&
+		    (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) {
 
 			/* Waypoints and gate are at the exact same position, which indicates an
 			 * invalid mission and makes calculating the direction from one waypoint

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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
@@ -37,6 +37,7 @@
 #include <mathlib/mathlib.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/Subscription.hpp>
 #include <px4_platform_common/module_params.h>
@@ -96,17 +97,18 @@ private:
 	uORB::Subscription _home_pos_sub{ORB_ID(home_position)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
 	// parameters
 	float _param_fw_lnd_ang{0.f};
 	float _param_mis_dist_1wp{0.f};
-	float _param_mis_dist_wps{0.f};
 	float _param_nav_acc_rad{0.f};
 	int32_t _param_mis_takeoff_land_req{0};
 
 	bool _is_landed{false};
 	float _home_alt_msl{NAN};
 	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
+	matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	VehicleType _vehicle_type{VehicleType::RotaryWing};
 
 	// internal flags to keep track of which checks failed
@@ -130,7 +132,7 @@ private:
 	int _landing_approach_index{-1};
 	mission_item_s _mission_item_previous = {};
 
-	// internal checkDistanceToFirstWaypoint variables
+	// internal checkHorizontalDistanceToFirstWaypoint variables
 	bool _first_waypoint_found{false};
 
 	// internal checkDistancesBetweenWaypoints variables
@@ -172,12 +174,12 @@ private:
 	bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index);
 
 	/**
-	 * @brief Check distance to first waypoint.
+	 * @brief Check distance to first waypoint from current vehicle position (if available).
 	 *
 	 * @param mission_item The current mission item
 	 * @return False if the check failed.
 	*/
-	bool checkDistanceToFirstWaypoint(mission_item_s &mission_item);
+	bool checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item);
 
 	/**
 	 * @brief Check distances between waypoints

src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2022-2023 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
@@ -33,8 +33,6 @@
 
 #include <gtest/gtest.h>
 #include "FeasibilityChecker.hpp"
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_land_detected.h>
 #include <lib/geo/geo.h>
 
 
@@ -67,6 +65,15 @@ public:
 		orb_publish(ORB_ID(home_position), home_pub, &home);
 	}
 
+	void publishCurrentPosition(double lat, double lon)
+	{
+		vehicle_global_position_s gpos = {};
+		gpos.lat = lat;
+		gpos.lon = lon;
+		orb_advert_t gpos_pub = orb_advertise(ORB_ID(vehicle_global_position), &gpos);
+		orb_publish(ORB_ID(vehicle_global_position), gpos_pub, &gpos);
+	}
+
 	void publishLanded(bool landed)
 	{
 		vehicle_land_detected_s land_detected = {};
@@ -123,73 +130,47 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity)
 	ASSERT_EQ(ret, false);
 }
 
-TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints)
-{
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-
-	param_t param = param_handle(px4::params::MIS_DIST_WPS);
-
-	float max_dist = 1000.0f;
-	param_set(param, &max_dist);
-	checker.paramsChanged();
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-
-	checker.processNextItem(mission_item, 0, 3);
-
-	// waypoints are within limits, check should pass
-	double lat_new, lon_new;
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 999, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 1, 3);
-
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// waypoints are above limits, check should fail
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1001, &lat_new, &lon_new);
-
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 2, 3);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-}
-
-
 TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
 {
+	// GIVEN: MIS_DIST_1WP set to 500m
 	TestFeasibilityChecker checker;
 	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
-
 	param_t param = param_handle(px4::params::MIS_DIST_1WP);
-
 	float max_dist = 500.0f;
 	param_set(param, &max_dist);
 	checker.paramsChanged();
-
-
 	mission_item_s mission_item = {};
 	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 	double lat_new, lon_new;
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
+
+	// GIVEN: no valid Current position
+
+	// THEN: always pass
 	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), false);
 
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
+	// BUT WHEN: valid current position, first WP 501m away from current pos
 	checker.reset();
 	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
+	checker.publishCurrentPosition(0, 0);
+	waypoint_from_heading_and_distance(0, 0, 0, 501, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+
+	// THEN: fail
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// BUT WHEN: valid current position fist WP 499m away from current
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishCurrentPosition(0, 0);
 	waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
 	mission_item.lat = lat_new;
 	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 0, 1);
 
+	// THEN: pass
+	checker.processNextItem(mission_item, 0, 1);
 	ASSERT_EQ(checker.someCheckFailed(), false);
 }
 

src/modules/navigator/mission.h

@@ -242,7 +242,6 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
-		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
 		(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
 	)
 

src/modules/navigator/mission_params.c

@@ -73,14 +73,14 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
 
 /**
- * Maximal horizontal distance from home to first waypoint
+ * Maximal horizontal distance from current position to first waypoint
  *
  * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
  * Set a value of zero or less to disable. The mission will not be started if the current
- * waypoint is more distant than MIS_DIST_1WP from the home position.
+ * waypoint is more distant than MIS_DIST_1WP from the current position.
  *
  * @unit m
- * @min 0
+ * @min -1
  * @max 10000
  * @decimal 1
  * @increment 100
@@ -88,22 +88,6 @@ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
  */
 PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
 
-/**
- * Maximal horizontal distance between waypoint
- *
- * Failsafe check to prevent running missions which are way too big.
- * Set a value of zero or less to disable. The mission will not be started if any distance between
- * two subsequent waypoints is greater than MIS_DIST_WPS.
- *
- * @unit m
- * @min 0
- * @max 10000
- * @decimal 1
- * @increment 100
- * @group Mission
- */
-PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900);
-
 /**
 * Enable yaw control of the mount. (Only affects multicopters and ROI mission items)
 *

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -181,7 +181,13 @@ void VehicleIMU::Run()
 	ScheduleDelayed(_backup_schedule_timeout_us);
 
 	// check vehicle status for changes to armed state
-	_vehicle_control_mode_armed_sub.update();
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+		}
+	}
 
 	// reset data gap monitor
 	_data_gap = false;
@@ -248,7 +254,7 @@ void VehicleIMU::Run()
 	}
 
 	if (_param_sens_imu_autocal.get() && !parameters_updated) {
-		if ((_vehicle_control_mode_armed_sub.get() || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
+		if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
 		    && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
 
 			SensorCalibrationUpdate();

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -109,8 +109,7 @@ private:
 	uORB::Subscription _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
-	uORB::SubscriptionSelection<&vehicle_control_mode_s::flag_armed>
-	_vehicle_control_mode_armed_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -142,7 +142,7 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 		_mag_comp_type = mag_comp_typ;
 
 
-		if (!_vehicle_control_mode_armed_sub.get()) {
+		if (!_armed) {
 			bool calibration_updated = false;
 
 			// update mag priority (CAL_MAGx_PRIO)
@@ -219,7 +219,7 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 					_calibration_estimator_bias[mag_index] = bias;
 
 					// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
-					if (_param_sens_mag_autocal.get() && !_vehicle_control_mode_armed_sub.get() && mag_bias_est.stable[mag_index]
+					if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
 					    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
 
 						const Vector3f old_offset = _calibration[mag_index].offset();
@@ -263,7 +263,7 @@ void VehicleMagnetometer::UpdateMagCalibration()
 	static constexpr float min_var_allowed = magb_vref * 0.01f;
 	static constexpr float max_var_allowed = magb_vref * 500.f;
 
-	if (_vehicle_control_mode_armed_sub.get()) {
+	if (_armed) {
 		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
 
 		for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) {
@@ -363,7 +363,7 @@ void VehicleMagnetometer::UpdatePowerCompensation()
 {
 	if (_mag_comp_type != MagCompensationType::Disabled) {
 		// update power signal for mag compensation
-		if (_vehicle_control_mode_armed_sub.get() && (_mag_comp_type == MagCompensationType::Throttle)) {
+		if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) {
 			vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
 
 			if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) {
@@ -403,8 +403,14 @@ void VehicleMagnetometer::Run()
 
 	const bool parameter_update = ParametersUpdate();
 
-	// update armed state
-	_vehicle_control_mode_armed_sub.update();
+	// check vehicle status for changes to armed state
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+		}
+	}
 
 	UpdatePowerCompensation();
 
@@ -563,7 +569,7 @@ void VehicleMagnetometer::Run()
 		CheckFailover(time_now_us);
 	}
 
-	if (!_vehicle_control_mode_armed_sub.get()) {
+	if (!_armed) {
 		calcMagInconsistency();
 	}
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -113,8 +113,7 @@ private:
 	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
 	uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)};
-	uORB::SubscriptionSelection<&vehicle_control_mode_s::flag_armed>
-	_vehicle_control_mode_armed_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 
 	// Used to check, save and use learned magnetometer biases
 	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
@@ -172,6 +171,8 @@ private:
 
 	int8_t _selected_sensor_sub_index{-1};
 
+	bool _armed{false};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
 		(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,

src/modules/simulation/battery_simulator/BatterySimulator.cpp

@@ -72,13 +72,19 @@ void BatterySimulator::Run()
 
 	updateCommands();
 
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
+
 	const hrt_abstime now_us = hrt_absolute_time();
 
 	const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;
 
-	_vehicle_status_arming_state_sub.update();
-
-	if (_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) {
+	if (_armed) {
 		if (_last_integration_us != 0) {
 			_battery_percentage -= (now_us - _last_integration_us) / discharge_interval_us;
 		}

src/modules/simulation/battery_simulator/BatterySimulator.hpp

@@ -77,7 +77,7 @@ private:
 	uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -86,6 +86,7 @@ private:
 
 	uint64_t _last_integration_us{0};
 	float _battery_percentage{1.f};
+	bool _armed{false};
 
 	bool _force_empty_battery{false};