diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index 64b9718608e3..e790f167361b 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/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 diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.cpp b/src/modules/vision_target_estimator/Position/VTEPosition.cpp index 554caa704240..864bbc594bed 100644 --- a/src/modules/vision_target_estimator/Position/VTEPosition.cpp +++ b/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(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; } diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.h b/src/modules/vision_target_estimator/Position/VTEPosition.h index 186590449807..2d7212bbc7a6 100644 --- a/src/modules/vision_target_estimator/Position/VTEPosition.h +++ b/src/modules/vision_target_estimator/Position/VTEPosition.h @@ -65,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -143,6 +144,7 @@ class VTEPosition: public ModuleParams uORB::Publication _vte_aid_gps_vel_target_pub{ORB_ID(vte_aid_gps_vel_target)}; uORB::Publication _vte_aid_gps_vel_uav_pub{ORB_ID(vte_aid_gps_vel_uav)}; uORB::Publication _vte_aid_fiducial_marker_pub{ORB_ID(vte_aid_fiducial_marker)}; + uORB::Publication _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")};