From b64860f9f89fc1e9d3cf5300b31b21b995cacc33 Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Tue, 9 Dec 2025 10:22:36 -0500 Subject: [PATCH] consolidated into update method; use default value in declaration Signed-off-by: dirksavage88 --- .../distance_sensor/vl53l1x/vl53l1x.cpp | 21 +++++++++---------- .../distance_sensor/vl53l1x/vl53l1x.hpp | 2 +- .../drivers/rangefinder/PX4Rangefinder.cpp | 21 +++---------------- .../drivers/rangefinder/PX4Rangefinder.hpp | 8 ++----- 4 files changed, 16 insertions(+), 36 deletions(-) diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp index 5eab279780..2c3b22d57b 100644 --- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp +++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp @@ -199,12 +199,13 @@ int VL53L1X::collect() uint8_t ret = 0; uint8_t rangeStatus = VL53L1X_RANGE_STATUS_OK; uint16_t distance_mm = 0; - + static constexpr float DOWNWARD_FACING_THETA = -1.57f; + static constexpr float RIGHT_PSI = 0.117809725; + static constexpr float LEFT_PSI = -0.117809725; perf_begin(_sample_perf); - - ret = VL53L1X_GetRangeStatus(&rangeStatus); + const int8_t quality = VL53L1X_GetRangeStatus(&rangeStatus); if ((ret != PX4_OK) | (rangeStatus == VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS)) { perf_count(_comms_errors); @@ -236,8 +237,8 @@ int VL53L1X::collect() float psi = 0.f; float theta = 0.f; - if (_sensor_orientation == (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING) { - theta = -1.57f; + if (_sensor_orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { + theta = DOWNWARD_FACING_THETA; } float phi = 0.f; @@ -245,11 +246,11 @@ int VL53L1X::collect() switch (roi_center[_zone_index]) { case VL53L1X_ROI_MID_RIGHT: - psi = 0.117809725; // Approximately 7 degrees; + psi = RIGHT_PSI; // Approximately 7 degrees; break; case VL53L1X_ROI_MID_LEFT: - psi = -0.117809725; + psi = LEFT_PSI; break; default: @@ -259,8 +260,8 @@ int VL53L1X::collect() Quaternionf quat_e = Eulerf(phi, theta, psi); float dist_q[] {quat_e(0), quat_e(1), quat_e(2), quat_e(3)}; - _px4_rangefinder.update_quaternion(dist_q); - _px4_rangefinder.update(timestamp_sample, distance_m); + + _px4_rangefinder.update(timestamp_sample, distance_m, quality, dist_q); return PX4_OK; } @@ -312,11 +313,9 @@ int VL53L1X::init() } // Spad width (x) & height (y) - uint8_t x = 8; uint8_t y = 16; - ret |= VL53L1X_SensorInit(); ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE); ret |= VL53L1X_SetROI(x, y); diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp index 6dd492cb36..2161bcf5b3 100644 --- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp +++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp @@ -157,7 +157,7 @@ private: int8_t VL53L1X_SetROICenter(uint8_t data); int8_t VL53L1X_SetROI(uint16_t x, uint16_t y); PX4Rangefinder _px4_rangefinder; - uint8_t _sensor_orientation{25}; + Rotation _sensor_orientation{ROTATION_PITCH_270}; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; }; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 9ccbd214b4..382fc863ed 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -68,18 +68,10 @@ void PX4Rangefinder::set_orientation(const uint8_t device_orientation) _distance_sensor_pub.get().orientation = device_orientation; } -void PX4Rangefinder::update_quaternion(const float (&quat)[4]) -{ - memcpy(_quat, quat, sizeof(float) * 4); - _q_update_now = hrt_absolute_time(); - _quat_updated = true; -} - -void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality) +void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality, const float (&quat)[4]) { distance_sensor_s &report = _distance_sensor_pub.get(); - report.timestamp = timestamp_sample; report.current_distance = distance; report.signal_quality = quality; @@ -91,15 +83,8 @@ void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float dis } } - // Update the quaternion in the sample update if we have set the update flag to true and the quaternion is fresh - if (_quat_updated && (_q_update_now - timestamp_sample < 10_ms)) { - for (uint8_t i = 0; i < 4; ++i) { - report.q[i] = _quat[i]; - - } - - } + // Update the quaternion in the sample update + memcpy(report.q, quat, sizeof(float) * 4); _distance_sensor_pub.update(); - _quat_updated = false; } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index 03829b145f..5b36299826 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -62,17 +62,13 @@ public: void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; } - // update quaterion sensor orientation with respect to the vehicle body frame (only called explicitly via method, not by the constructor when object created) - void update_quaternion(const float (&quat)[4]); - - void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality = -1); + // update with quaterion sensor orientation with respect to the vehicle body frame + void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality = -1, const float (&quat)[4] = {1.0, 0.0, 0.0, 0.0}); int get_instance() { return _distance_sensor_pub.get_instance(); }; uint32_t get_device_id() { return _distance_sensor_pub.get().device_id; }; private: uORB::PublicationMultiData _distance_sensor_pub{ORB_ID(distance_sensor)}; - bool _quat_updated{false}; - float _quat[4] {1.0, 0.0, 0.0, 0.0}; hrt_abstime _q_update_now; };