wip add support for UWB

msg/EstimatorAidSource3d.msg

@@ -23,4 +23,4 @@ bool fused                      # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag
 # TOPICS vte_aid_gps_pos_target vte_aid_gps_pos_mission vte_aid_gps_vel_target vte_aid_gps_vel_uav
-# TOPICS vte_aid_fiducial_marker
+# TOPICS vte_aid_fiducial_marker vte_aid_uwb

src/modules/vision_target_estimator/Position/VTEPosition.cpp

@@ -61,6 +61,7 @@ VTEPosition::VTEPosition() :
 	_vte_aid_gps_vel_uav_pub.advertise();
 	_vte_aid_gps_vel_target_pub.advertise();
 	_vte_aid_fiducial_marker_pub.advertise();
+	_vte_aid_uwb_pub.advertise();
 
 	_check_params(true);
 }
@@ -322,6 +323,9 @@ void VTEPosition::processObservations(ObservationValidMask &vte_fusion_aid_mask,
 	/* Handle Vision Data */
 	handleVisionData(vte_fusion_aid_mask, observations[ObservationType::fiducial_marker]);
 
+	/* Handle UWB Data */
+	handleUwbData(vte_fusion_aid_mask, observations[ObservationType::uwb]);
+
 	/* Handle UAV GPS position */
 	sensor_gps_s vehicle_gps_position;
 	const bool vehicle_gps_position_updated = _vehicle_gps_position_sub.update(&vehicle_gps_position);
@@ -371,6 +375,76 @@ bool VTEPosition::isVisionDataValid(const fiducial_marker_pos_report_s &fiducial
 }
 
 
+void VTEPosition::handleUwbData(ObservationValidMask &vte_fusion_aid_mask, targetObsPos &obs_uwb)
+{
+
+	sensor_uwb_s uwb_report;
+
+	if (!((_vte_aid_mask & SensorFusionMask::USE_UWB)
+	      && _sensor_uwb_sub.update(&uwb_report))) {
+		return;
+	}
+
+	if (!isUwbDataValid(uwb_report)) {
+		return;
+	}
+
+	if (processObsUwb(uwb_report, obs_uwb)) {
+		vte_fusion_aid_mask = static_cast<ObservationValidMask>(vte_fusion_aid_mask | ObservationValidMask::FUSE_UWB);
+	}
+
+}
+
+
+bool VTEPosition::isUwbDataValid(const sensor_uwb_s &uwb_report)
+{
+
+	// TODO: extend checks
+	return _is_meas_valid(uwb_report.timestamp);
+
+}
+
+
+bool VTEPosition::processObsUwb(const sensor_uwb_s &uwb_report, targetObsPos &obs)
+{
+
+	// Convert degrees to radians
+	const float theta_rad = uwb_report.aoa_azimuth_dev * (M_PIf32 / 180.0f);
+	const float phi_rad = uwb_report.aoa_elevation_dev * (M_PIf32 / 180.0f);
+
+	const float distance = uwb_report.distance;
+
+	// Calculate the relative position components
+	const float delta_x = distance * cosf(phi_rad) * cosf(theta_rad);
+	const float delta_y = distance * cosf(phi_rad) * sinf(theta_rad);
+	const float delta_z = -distance * sinf(phi_rad); // Negative because Z is down in NED
+
+	// Total position in NED frame
+	const Vector3f pos_ned(uwb_report.offset_x + delta_x, uwb_report.offset_y + delta_y, uwb_report.offset_z + delta_z);
+
+	obs.meas_xyz = pos_ned;
+
+	obs.meas_h_xyz(Direction::x, vtest::State::pos_rel.idx) = 1;
+	obs.meas_h_xyz(Direction::y, vtest::State::pos_rel.idx) = 1;
+	obs.meas_h_xyz(Direction::z, vtest::State::pos_rel.idx) = 1;
+
+	const float unc = math::max(math::min(distance / 20.f, 2.f), 0.1f);
+
+	obs.meas_unc_xyz(Direction::x) = unc;
+	obs.meas_unc_xyz(Direction::y) = unc;
+	obs.meas_unc_xyz(Direction::z) = unc;
+
+	obs.timestamp = uwb_report.timestamp;
+
+	obs.type = ObservationType::uwb;
+	obs.updated = true;
+
+
+	// TODO: fix function and then return true.
+	return false;
+}
+
+
 void VTEPosition::handleUavGpsData(const sensor_gps_s &vehicle_gps_position,
 				   ObservationValidMask &vte_fusion_aid_mask,
 				   targetObsPos &obs_gps_pos_mission,
@@ -1047,6 +1121,10 @@ void VTEPosition::publishInnov(const estimator_aid_source3d_s &target_innov, con
 		_vte_aid_fiducial_marker_pub.publish(target_innov);
 		break;
 
+	case ObservationType::uwb:
+		_vte_aid_uwb_pub.publish(target_innov);
+		break;
+
 	case ObservationType::nb_observation_types:
 		break;
 	}

src/modules/vision_target_estimator/Position/VTEPosition.h

@@ -65,6 +65,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_uwb.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <matrix/Matrix.hpp>
@@ -143,6 +144,7 @@ class VTEPosition: public ModuleParams
 	uORB::Publication<estimator_aid_source3d_s> _vte_aid_gps_vel_target_pub{ORB_ID(vte_aid_gps_vel_target)};
 	uORB::Publication<estimator_aid_source3d_s> _vte_aid_gps_vel_uav_pub{ORB_ID(vte_aid_gps_vel_uav)};
 	uORB::Publication<estimator_aid_source3d_s> _vte_aid_fiducial_marker_pub{ORB_ID(vte_aid_fiducial_marker)};
+	uORB::Publication<estimator_aid_source3d_s> _vte_aid_uwb_pub{ORB_ID(vte_aid_uwb)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -166,7 +168,8 @@ class VTEPosition: public ModuleParams
 		uav_gps_vel = 2,
 		target_gps_vel = 3,
 		fiducial_marker = 4,
-		nb_observation_types = 5
+		uwb = 5,
+		nb_observation_types = 6
 	};
 
 	struct targetObsPos {
@@ -189,6 +192,7 @@ class VTEPosition: public ModuleParams
 		USE_EXT_VIS_POS 	= (1 << 2),    ///< set to true to use target external vision-based relative position data
 		USE_MISSION_POS     = (1 << 3),    ///< set to true to use the PX4 mission position
 		USE_TARGET_GPS_VEL  = (1 << 4),		///< set to true to use target GPS velocity data. Only for moving targets.
+		USE_UWB = (1 << 5) ///< set to true to use UWB.
 	};
 
 	enum ObservationValidMask : uint8_t {
@@ -199,6 +203,7 @@ class VTEPosition: public ModuleParams
 		FUSE_EXT_VIS_POS 	  = (1 << 2),    ///< set to true if target external vision-based relative position data is ready to be fused
 		FUSE_MISSION_POS     = (1 << 3),    ///< set to true if the PX4 mission position is ready to be fused
 		FUSE_TARGET_GPS_VEL     = (1 << 4),   ///< set to true if target GPS velocity data is ready to be fused
+		FUSE_UWB 				= (1 << 5) ///< set to true if UWB data is ready to be fused
 	};
 
 	bool initTargetEstimator();
@@ -248,6 +253,11 @@ class VTEPosition: public ModuleParams
 	bool isVisionDataValid(const fiducial_marker_pos_report_s &fiducial_marker_pose);
 	bool processObsVision(const fiducial_marker_pos_report_s &fiducial_marker_pose, targetObsPos &obs);
 
+	/* UWB data */
+	void handleUwbData(ObservationValidMask &vte_fusion_aid_mask, targetObsPos &obs_uwb);
+	bool isUwbDataValid(const sensor_uwb_s &uwb_report);
+	bool processObsUwb(const sensor_uwb_s &uwb_report, targetObsPos &obs);
+
 	/* UAV GPS data */
 	void handleUavGpsData(const sensor_gps_s &vehicle_gps_position,
 			      ObservationValidMask &vte_fusion_aid_mask,
@@ -279,6 +289,7 @@ class VTEPosition: public ModuleParams
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _fiducial_marker_report_sub{ORB_ID(fiducial_marker_pos_report)};
 	uORB::Subscription _target_gnss_sub{ORB_ID(target_gnss)};
+	uORB::Subscription _sensor_uwb_sub{ORB_ID(sensor_uwb)};
 
 	perf_counter_t _vte_predict_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE prediction")};
 	perf_counter_t _vte_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE update")};