Support for NXP UWB position sensor

uwb_sr150 driver for the sensor, and some
modifications in precision landing to allow
landing on a platform using the UWB system.

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -100,58 +100,21 @@ void LandingTargetEstimator::update()
 		}
 	}
 
-	if (!_new_irlockReport) {
+	if (!_new_sensorReport) {
 		// nothing to do
 		return;
 	}
 
 	// mark this sensor measurement as consumed
-	_new_irlockReport = false;
+	_new_sensorReport = false;
 
-	if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
-		// don't have the data needed for an update
-		return;
-	}
-
-	if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
-		return;
-	}
-
-	matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
-	sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
-	sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
-	sensor_ray(2) = 1.0f;
-
-	// rotate unit ray according to sensor orientation
-	_S_att = get_rot_matrix(_params.sensor_yaw);
-	sensor_ray = _S_att * sensor_ray;
-
-	// rotate the unit ray into the navigation frame
-	matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
-	_R_att = matrix::Dcm<float>(q_att);
-	sensor_ray = _R_att * sensor_ray;
-
-	if (fabsf(sensor_ray(2)) < 1e-6f) {
-		// z component of measurement unsafe, don't use this measurement
-		return;
-	}
-
-	float dist = _vehicleLocalPosition.dist_bottom - _params.offset_z;
-
-	// scale the ray s.t. the z component has length of dist
-	_rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist;
-	_rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist;
-
-	// Adjust relative position according to sensor offset
-	_rel_pos(0) += _params.offset_x;
-	_rel_pos(1) += _params.offset_y;
 
 	if (!_estimator_initialized) {
-		PX4_INFO("Init");
 		float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
 		float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
-		_kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init);
-		_kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init);
+		PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init);
+		_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init);
+		_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init);
 
 		_estimator_initialized = true;
 		_last_update = hrt_absolute_time();
@@ -159,8 +122,9 @@ void LandingTargetEstimator::update()
 
 	} else {
 		// update
-		bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist);
-		bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist);
+		const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z;
+		bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty);
+		bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
 
 		if (!update_x || !update_y) {
 			if (!_faulty) {
@@ -175,7 +139,7 @@ void LandingTargetEstimator::update()
 		if (!_faulty) {
 			// only publish if both measurements were good
 
-			_target_pose.timestamp = _irlockReport.timestamp;
+			_target_pose.timestamp = _target_position_report.timestamp;
 
 			float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
 			_kalman_filter_x.getState(x, xvel);
@@ -190,7 +154,7 @@ void LandingTargetEstimator::update()
 			_target_pose.rel_vel_valid = true;
 			_target_pose.x_rel = x;
 			_target_pose.y_rel = y;
-			_target_pose.z_rel = dist;
+			_target_pose.z_rel = _target_position_report.rel_pos_z ;
 			_target_pose.vx_rel = xvel;
 			_target_pose.vy_rel = yvel;
 
@@ -203,7 +167,7 @@ void LandingTargetEstimator::update()
 			if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
 				_target_pose.x_abs = x + _vehicleLocalPosition.x;
 				_target_pose.y_abs = y + _vehicleLocalPosition.y;
-				_target_pose.z_abs = dist + _vehicleLocalPosition.z;
+				_target_pose.z_abs = _target_position_report.rel_pos_z  + _vehicleLocalPosition.z;
 				_target_pose.abs_pos_valid = true;
 
 			} else {
@@ -220,7 +184,7 @@ void LandingTargetEstimator::update()
 		_kalman_filter_x.getInnovations(innov_x, innov_cov_x);
 		_kalman_filter_y.getInnovations(innov_y, innov_cov_y);
 
-		_target_innovations.timestamp = _irlockReport.timestamp;
+		_target_innovations.timestamp = _target_position_report.timestamp;
 		_target_innovations.innov_x = innov_x;
 		_target_innovations.innov_cov_x = innov_cov_x;
 		_target_innovations.innov_y = innov_y;
@@ -246,7 +210,76 @@ void LandingTargetEstimator::_update_topics()
 	_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
 	_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
 
-	_new_irlockReport = _irlockReportSub.update(&_irlockReport);
+
+	if (_irlockReportSub.update(&_irlockReport)) { //
+		_new_irlockReport = true;
+
+		if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
+			// don't have the data needed for an update
+			return;
+		}
+
+		if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
+			return;
+		}
+
+		matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
+		sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
+		sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
+		sensor_ray(2) = 1.0f;
+
+		// rotate unit ray according to sensor orientation
+		_S_att = get_rot_matrix(_params.sensor_yaw);
+		sensor_ray = _S_att * sensor_ray;
+
+		// rotate the unit ray into the navigation frame
+		matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
+		_R_att = matrix::Dcm<float>(q_att);
+		sensor_ray = _R_att * sensor_ray;
+
+		if (fabsf(sensor_ray(2)) < 1e-6f) {
+			// z component of measurement unsafe, don't use this measurement
+			return;
+		}
+
+		_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
+
+		// scale the ray s.t. the z component has length of _uncertainty_scale
+		_target_position_report.timestamp = _irlockReport.timestamp;
+		_target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z;
+		_target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z;
+		_target_position_report.rel_pos_z = _dist_z;
+
+		// Adjust relative position according to sensor offset
+		_target_position_report.rel_pos_x += _params.offset_x;
+		_target_position_report.rel_pos_y += _params.offset_y;
+
+		_new_sensorReport = true;
+
+	} else if (_uwbDistanceSub.update(&_uwbDistance)) {
+		if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
+			// don't have the data needed for an update
+			PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
+			return;
+		}
+
+		if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) ||
+		    !PX4_ISFINITE((float)_uwbDistance.position[2])) {
+			PX4_WARN("Position is corrupt!");
+			return;
+		}
+
+		_new_sensorReport = true;
+
+		// The coordinate system is NED (north-east-down)
+		// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
+		// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
+		// To change POV we negate every Axis:
+		_target_position_report.timestamp = _uwbDistance.timestamp;
+		_target_position_report.rel_pos_x = -_uwbDistance.position[0];
+		_target_position_report.rel_pos_y = -_uwbDistance.position[1];
+		_target_position_report.rel_pos_z = -_uwbDistance.position[2];
+	}
 }
 
 void LandingTargetEstimator::_update_params()

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -54,6 +54,9 @@
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/landing_target_innovations.h>
+#include <uORB/topics/uwb_distance.h>
+#include <uORB/topics/uwb_grid.h>
+#include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -139,21 +142,32 @@ private:
 		enum Rotation sensor_yaw;
 	} _params;
 
+	struct {
+		hrt_abstime timestamp;
+		float rel_pos_x;
+		float rel_pos_y;
+		float rel_pos_z;
+	} _target_position_report;
+
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
+	uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
 
 	vehicle_local_position_s	_vehicleLocalPosition{};
 	vehicle_attitude_s		_vehicleAttitude{};
 	vehicle_acceleration_s		_vehicle_acceleration{};
 	irlock_report_s			_irlockReport{};
+	uwb_grid_s		_uwbGrid{};
+	uwb_distance_s		_uwbDistance{};
 
 	// keep track of which topics we have received
 	bool _vehicleLocalPosition_valid{false};
 	bool _vehicleAttitude_valid{false};
 	bool _vehicle_acceleration_valid{false};
 	bool _new_irlockReport{false};
+	bool _new_sensorReport{false};
 	bool _estimator_initialized{false};
 	// keep track of whether last measurement was rejected
 	bool _faulty{false};
@@ -165,11 +179,10 @@ private:
 	KalmanFilter _kalman_filter_y;
 	hrt_abstime _last_predict{0}; // timestamp of last filter prediction
 	hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
+	float _dist_z{1.0f};
 
 	void _check_params(const bool force);
 
 	void _update_state();
 };
-
-
 } // namespace landing_target_estimator

src/modules/landing_target_estimator/landing_target_estimator_main.cpp

@@ -98,7 +98,7 @@ int landing_target_estimator_main(int argc, char *argv[])
 		daemon_task = px4_task_spawn_cmd("landing_target_estimator",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_DEFAULT,
-						 2000,
+						 2100,
 						 landing_target_estimator_thread_main,
 						 (argv) ? (char *const *)&argv[2] : nullptr);
 		return 0;

src/modules/navigator/precland.cpp

@@ -256,21 +256,13 @@ PrecLand::run_state_horizontal_approach()
 		if (hrt_absolute_time() - _point_reached_time > 2000000) {
 			// if close enough for descent above target go to descend above target
 			if (switch_to_state_descend_above_target()) {
+
 				return;
 			}
 		}
 
 	}
 
-	if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) {
-		PX4_ERR("Precision landing took too long during horizontal approach phase.");
-
-		if (switch_to_state_fallback()) {
-			return;
-		}
-
-		PX4_ERR("Can't switch to fallback landing");
-	}
 
 	float x = _target_pose.x_abs;
 	float y = _target_pose.y_abs;
@@ -388,6 +380,7 @@ bool
 PrecLand::switch_to_state_horizontal_approach()
 {
 	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
+		PX4_INFO("Precland: switching to horizontal approach!");
 		_approach_alt = _navigator->get_global_position()->alt;
 
 		_point_reached_time = 0;
@@ -404,6 +397,7 @@ bool
 PrecLand::switch_to_state_descend_above_target()
 {
 	if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
+		PX4_INFO("Precland: switching to descend!");
 		_state = PrecLandState::DescendAboveTarget;
 		_state_start_time = hrt_absolute_time();
 		return true;
@@ -416,6 +410,7 @@ bool
 PrecLand::switch_to_state_final_approach()
 {
 	if (check_state_conditions(PrecLandState::FinalApproach)) {
+		PX4_INFO("Precland: switching ot final approach");
 		_state = PrecLandState::FinalApproach;
 		_state_start_time = hrt_absolute_time();
 		return true;