From f8b8de24661c45b0d53ae5d368f0bc7e6f3ea532 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 22 Aug 2018 20:37:48 +0200 Subject: [PATCH] Fix units; Check sanity --- src/main/io/rangefinder_benewake.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/io/rangefinder_benewake.c b/src/main/io/rangefinder_benewake.c index f8e243deb20..d7fdc9b14e6 100644 --- a/src/main/io/rangefinder_benewake.c +++ b/src/main/io/rangefinder_benewake.c @@ -54,6 +54,7 @@ typedef struct __attribute__((packed)) { } tfminiPacket_t; #define BENEWAKE_PACKET_SIZE sizeof(tfminiPacket_t) +#define BENEWAKE_MIN_QUALITY 0 static serialPort_t * serialPort = NULL; static serialPortConfig_t * portConfig; @@ -118,6 +119,20 @@ static void benewakeRangefinderUpdate(void) hasNewData = true; sensorData = (((tfminiPacket_t *) &buffer[0])->distL << 0) | (((tfminiPacket_t *) &buffer[0])->distH << 8); + + uint16_t qual = (((tfminiPacket_t *) &buffer[0])->strengthL << 0) | + (((tfminiPacket_t *) &buffer[0])->strengthH << 8); + + if (sensorData == 0 || qual <= BENEWAKE_MIN_QUALITY) { + sensorData = -1; + } + + /* + debug[0] = ((tfminiPacket_t *) &buffer[0])->distL; + debug[1] = ((tfminiPacket_t *) &buffer[0])->distH; + debug[2] = ((tfminiPacket_t *) &buffer[0])->strengthH; + debug[3] = ((tfminiPacket_t *) &buffer[0])->strengthL; + */ } // Prepare for new packet @@ -130,7 +145,7 @@ static int32_t benewakeRangefinderGetDistance(void) { if (hasNewData) { hasNewData = false; - return (sensorData > 0) ? (sensorData / 10) : RANGEFINDER_OUT_OF_RANGE; + return (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE; } else { return RANGEFINDER_NO_NEW_DATA;