dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VL53L1X: fix scheduling (#21787) 

* should be 50 Hz instead of 50,000 Hz

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
Andrew Brahim 2023-07-03 10:49:53 -04:00 committed by GitHub
parent a36c922fc0
commit 24665f10f2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
View File

@ -35,10 +35,11 @@
***********/
#include "vl53l1x.hpp"
#include <px4_platform_common/module.h>
#define VL53L1X_DELAY 20 //ms
#define VL53L1X_DELAY 50000 // delay to reduce CPU usage (us)
#define VL53L1X_SAMPLE_RATE 200 // ms, default
#define VL53L1X_INTER_MEAS_MS 200 // ms
#define VL53L1X_INTER_MEAS_MS 200 // ms
#define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode
#define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode
#define VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS 13 // region of interest out of bounds error
@ -261,18 +262,19 @@ int VL53L1X::probe()
void VL53L1X::RunImpl()
{
uint8_t dataReady = 0;
VL53L1X_CheckForDataReady(&dataReady);
if (dataReady == 1) {
collect();
}
// Reduce CPU usage
ScheduleDelayed(VL53L1X_DELAY);
// zone modulus increment
_zone_index = (_zone_index + 1) % _zone_limit;
// Set the ROI center based on zone incrementation
VL53L1X_SetROICenter(roi_center[_zone_index]);
}
@ -292,6 +294,7 @@ int VL53L1X::init()
uint8_t x = 4;
uint8_t y = 4;
ret |= VL53L1X_SensorInit();
ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE);
ret |= VL53L1X_SetROI(x, y);
@ -488,6 +491,7 @@ int8_t VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady)
uint8_t IntPol;
int8_t status = 0;
status = VL53L1X_GetInterruptPolarity(&IntPol);
status = VL53L1_RdByte(GPIO__TIO_HV_STATUS, &Temp);
@ -498,6 +502,7 @@ int8_t VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady)
} else {
*isDataReady = 0;
}
}
@ -520,6 +525,7 @@ int8_t VL53L1X::VL53L1X_ClearInterrupt()
int8_t status = 0;
status = VL53L1_WrByte(SYSTEM__INTERRUPT_CLEAR, 0x01);
return status;
}
@ -528,6 +534,8 @@ int8_t VL53L1X::VL53L1X_StopRanging()
int8_t status = 0;
status = VL53L1_WrByte(SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */
ScheduleClear();
return status;
}