Log camera topics

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

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))

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_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/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)
 *