37 lines
677 B
C
37 lines
677 B
C
|
#include "by_measure.h"
|
||
|
|
|
||
|
|
#include "by_debug.h"
|
||
|
|
#include "vl53l0x_wmd_api.h"
|
||
|
|
|
||
|
|
void by_measure_init(void)
|
||
|
|
{
|
||
|
|
VL53L0X_Init();
|
||
|
|
}
|
||
|
|
|
||
|
|
void by_measure_run(void)
|
||
|
|
{
|
||
|
|
float x = VL53L0X_GetValue();
|
||
|
|
float distance = x;
|
||
|
|
static float distance_last = 0;
|
||
|
|
|
||
|
|
if (x < 270) {
|
||
|
|
distance = x - 80.0f;
|
||
|
|
} else if (x < 330) {
|
||
|
|
distance = 2.0755f * x - 368.85f;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 错误值
|
||
|
|
if (distance <= 20 || distance > 330) {
|
||
|
|
distance = distance_last;
|
||
|
|
}
|
||
|
|
|
||
|
|
// if (distance <= 20) {
|
||
|
|
// distance = distance_last;
|
||
|
|
// } else if (distance > 330) {
|
||
|
|
// distance = 330;
|
||
|
|
// }
|
||
|
|
|
||
|
|
distance_last = distance;
|
||
|
|
|
||
|
|
LOGI("distance:%f", distance);
|
||
|
|
}
|