fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 03:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers: rework NXP UWB driver (#21124)

Browse Source 
* UWB driver rework that uses 2 UWB MKBoards - 1 as Controller (Initiator), one as Controllee (Anchor)

Co-authored-by: NXPBrianna <108274268+NXPBrianna@users.noreply.github.com>
This commit is contained in:
Loic Fernau
2023-07-12 11:44:23 -04:00
committed by GitHub
co-authored by NXPBrianna
parent 715a1ff701
commit f8c9be087b
12 changed files with 316 additions and 693 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+3 -26
View File
@@ -100,13 +100,13 @@ void LandingTargetEstimator::update()
}
}
if (!_new_sensorReport) {
if (!_new_irlockReport) {
// nothing to do
return;
}
// mark this sensor measurement as consumed
_new_sensorReport = false;
_new_irlockReport = false;
if (!_estimator_initialized) {
@@ -254,30 +254,7 @@ void LandingTargetEstimator::_update_topics()
_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 (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
PX4_WARN("Marker position reading invalid!");
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];
_new_irlockReport = true;
}
}