fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 14:43:33 +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;
}
}
src/modules/landing_target_estimator/LandingTargetEstimator.h
-6
View File
@@ -54,9 +54,6 @@
#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>
@@ -153,14 +150,11 @@ private:
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};