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

consolidated into update method; use default value in declaration

Browse Source 
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
dirksavage88
2025-12-17 17:12:12 +01:00
committed by Matthias Grob
parent e51d09612f
commit b64860f9f8
4 changed files with 16 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
+10 -11
View File
@@ -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);
src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp
+1 -1
View File
@@ -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")};
};