Add local path setpoint

Pass setpoints to npfg


F
F
F

ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer

@@ -44,6 +44,8 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
 
+param set-default FW_USE_NPFG 1
+
 param set-default RWTO_TKOFF 1
 
 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix

msg/CMakeLists.txt

@@ -185,6 +185,7 @@ set(msg_files
 	vehicle_land_detected.msg
 	vehicle_local_position.msg
 	vehicle_local_position_setpoint.msg
+	vehicle_local_path_setpoint.msg
 	vehicle_magnetometer.msg
 	vehicle_odometry.msg
 	vehicle_rates_setpoint.msg

msg/vehicle_local_path_setpoint.msg

@@ -0,0 +1,15 @@
+# Local position setpoint in NED frame
+# setting something to NaN means the state should not be controlled
+
+uint64 timestamp	# time since system start (microseconds)
+
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+float32 vx		# in meters/sec
+float32 vy		# in meters/sec
+float32 vz		# in meters/sec
+float32 airspeed	# air relative setpoint
+float32 curvature	# in meters/sec^2
+
+# TOPICS vehicle_local_path_setpoint

src/lib/npfg/npfg.cpp

@@ -617,6 +617,25 @@ void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle
 	updateRollSetpoint();
 } // navigateLoiter
 
+
+void NPFG::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
+			       const matrix::Vector2f &tangent_setpoint,
+			       const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
+{
+	path_type_loiter_ = false;
+
+	// set unit tangent directly
+	unit_path_tangent_ = tangent_setpoint.normalized();
+
+	// closest point to vehicle
+	matrix::Vector2f error_vector = position_setpoint - vehicle_pos;
+	signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
+
+	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
+
+	updateRollSetpoint();
+} // navigatePathTangent
+
 void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = false;

src/lib/npfg/npfg.hpp

@@ -248,6 +248,21 @@ public:
 			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);
 
+	/*
+	 * Path following logic. Takes poisiton, path tangent, curvature and
+	 * then updates control setpoints to follow a path setpoint.
+	 *
+	 * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] curvature of the path setpoint [1/m]
+	 */
+	void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
+				 const matrix::Vector2f &tangent_setpoint,
+				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
+
 	/*
 	 * Navigate on a fixed heading.
 	 *

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -795,7 +795,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 void
 FixedwingPositionControl::control_auto(const hrt_abstime &now, 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)
+				       const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next, vehicle_local_path_setpoint_s &path_sp)
 {
 	const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
 	_control_position_last_called = now;
@@ -885,11 +885,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		path_sp = control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		path_sp = control_auto_velocity(now, dt, ground_speed, current_sp);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
@@ -905,6 +905,15 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		break;
 	}
 
+	Vector2f path_point_sp(path_sp.x, path_sp.y);
+	Vector2f unit_path_tangent(path_sp.vx, path_sp.vy);
+	float curvature = path_sp.curvature;
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	_npfg.navigatePathTangent(curr_pos_local, path_point_sp, unit_path_tangent, get_nav_speed_2d(ground_speed), _wind_vel,
+				  curvature);
+
+	_att_sp.roll_body = _npfg.getRollSetpoint();
+
 	/* reset landing state */
 	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
 		reset_landing_state();
@@ -1101,7 +1110,7 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 	return position_sp_type;
 }
 
-void
+vehicle_local_path_setpoint_s
 FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
@@ -1187,12 +1196,12 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 	}
 
 	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+	float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
+			  0.0f;
 
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-		_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed), _wind_vel);
-		_att_sp.roll_body = _npfg.getRollSetpoint();
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
@@ -1211,11 +1220,23 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 				   tecs_fw_mission_throttle,
 				   false,
 				   radians(_param_fw_p_lim_min.get()));
+	/* populate l1 control setpoint */
+	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+	vehicle_local_path_setpoint_s setpoint;
+	setpoint.x = curr_wp_local(0);
+	setpoint.y = curr_wp_local(1);
+	setpoint.z = position_sp_alt;
+	setpoint.vx = pos_sp_curr.vx;
+	setpoint.vy = pos_sp_curr.vy;
+	setpoint.vz = pos_sp_curr.vz;
+	setpoint.airspeed = target_airspeed;
+	setpoint.curvature = curvature;
+	return setpoint;
 }
 
-void
-FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+vehicle_local_path_setpoint_s
+FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2f &ground_speed,
+		const position_setpoint_s &pos_sp_curr)
 {
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
@@ -1276,6 +1297,16 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 				   radians(_param_fw_p_lim_min.get()),
 				   tecs_status_s::TECS_MODE_NORMAL,
 				   pos_sp_curr.vz);
+	vehicle_local_path_setpoint_s setpoint;
+	setpoint.x = NAN;
+	setpoint.y = NAN;
+	setpoint.z = NAN;
+	setpoint.vx = pos_sp_curr.vx;
+	setpoint.vy = pos_sp_curr.vy;
+	setpoint.vz = pos_sp_curr.vz;
+	setpoint.airspeed = target_airspeed;
+	setpoint.curvature = 0.0f;
+	return setpoint;
 }
 
 void
@@ -2215,48 +2246,55 @@ FixedwingPositionControl::Run()
 			_global_local_alt0 = _local_pos.ref_alt;
 		}
 
+		vehicle_local_path_setpoint_s path_sp;
+
 		if (_control_mode.flag_control_offboard_enabled) {
 			vehicle_local_position_setpoint_s trajectory_setpoint;
 
 			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+				bool valid_setpoint = false;
+				path_sp = {}; // clear any existing
+				path_sp.timestamp = trajectory_setpoint.timestamp;
+				path_sp.vx = NAN;
+				path_sp.vy = NAN;
+				path_sp.vz = NAN;
+				path_sp.x = NAN;
+				path_sp.y = NAN;
+				path_sp.z = NAN;
+				path_sp.curvature = NAN;
+
 				if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
-					double lat;
-					double lon;
+					valid_setpoint = true;
+					path_sp.x = trajectory_setpoint.x;
+					path_sp.y = trajectory_setpoint.y;
+					path_sp.z = _global_local_alt0 - trajectory_setpoint.z;
+				}
 
-					if (_global_local_proj_ref.isInitialized()) {
-						_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
-						_pos_sp_triplet = {}; // clear any existing
+				if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
+				    && PX4_ISFINITE(trajectory_setpoint.vz)) {
+					valid_setpoint = true;
+					path_sp.vx = trajectory_setpoint.vx;
+					path_sp.vy = trajectory_setpoint.vy;
+					path_sp.vz = trajectory_setpoint.vz;
+
+					if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
+					    && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
+						Vector2f velocity_sp_2d(trajectory_setpoint.vx, trajectory_setpoint.vy);
+						Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
+						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) *
+									       velocity_sp_2d.normalized();
+						path_sp.curvature = acceleration_normal.norm() / (velocity_sp_2d.norm() * velocity_sp_2d.norm());
 
-						_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
-						_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
-						_pos_sp_triplet.current.valid = true;
-						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-						_pos_sp_triplet.current.lat = lat;
-						_pos_sp_triplet.current.lon = lon;
-						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
-						_pos_sp_triplet.current.cruising_speed = NAN; // ignored
-						_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
 					}
+				}
 
-				} else if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
-					   && PX4_ISFINITE(trajectory_setpoint.vz)) {
-					_pos_sp_triplet = {}; // clear any existing
-
-					_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
-					_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
-					_pos_sp_triplet.current.valid = true;
-					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-					_pos_sp_triplet.current.vx = trajectory_setpoint.vx;
-					_pos_sp_triplet.current.vy = trajectory_setpoint.vy;
-					_pos_sp_triplet.current.vz = trajectory_setpoint.vz;
-					_pos_sp_triplet.current.alt = NAN;
-					_pos_sp_triplet.current.cruising_speed = NAN; // ignored
-					_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
-
-				} else {
+				if (!valid_setpoint) {
 					mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t");
 					events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error,
 						     "Invalid offboard setpoint");
+
+				} else {
+					_pos_sp_triplet.current.valid = valid_setpoint;
 				}
 			}
 
@@ -2292,7 +2330,7 @@ FixedwingPositionControl::Run()
 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
 				control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
-					     _pos_sp_triplet.next);
+					     _pos_sp_triplet.next, path_sp);
 				break;
 			}
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -88,6 +88,7 @@
 #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_path_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
@@ -343,20 +344,19 @@ private:
 	uint8_t		handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
 	void		control_auto(const hrt_abstime &now, 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);
+				     const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next, vehicle_local_path_setpoint_s &path_sp);
 
 	void		control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
 	void		control_auto_descend(const hrt_abstime &now);
 
-	void		control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					      const Vector2f &ground_speed,
-					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	vehicle_local_path_setpoint_s		control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+			const Vector2f &ground_speed,
+			const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
 	void		control_auto_loiter(const hrt_abstime &now, const float dt, 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);
-	void		control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					      const Vector2f &ground_speed,
-					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	vehicle_local_path_setpoint_s		control_auto_velocity(const hrt_abstime &now, const float dt,
+			const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr);
 	void		control_auto_takeoff(const hrt_abstime &now, const float dt,  const Vector2d &curr_pos,
 					     const Vector2f &ground_speed,
 					     const position_setpoint_s &pos_sp_prev,