diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index 5766ac219d8d5e..df742fda80ede1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -13,11 +13,13 @@ along with this program. If not, see . */ -#include "AP_RangeFinder_JRE_Serial.h" -#include +#include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_JRE_SERIAL_ENABLED +#include "AP_RangeFinder_JRE_Serial.h" +#include + #define FRAME_HEADER_1 'R' // 0x52 #define FRAME_HEADER_2 'A' // 0x41 @@ -30,15 +32,10 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) if (uart == nullptr) { return false; // not update } - uint32_t n = uart->available(); - if (n == 0) { - return false; - } uint16_t valid_count = 0; // number of valid readings uint16_t invalid_count = 0; // number of invalid readings float sum = 0; - float reading_cm; // max distance the sensor can reliably measure - read from parameters const int16_t distance_cm_max = max_distance_cm(); @@ -71,17 +68,12 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) if (data_buff_idx >= ARRAY_SIZE(data_buff)) { // 1 data set complete // crc check uint16_t crc = crc16_ccitt_r(data_buff, ARRAY_SIZE(data_buff) - 2, 0xffff, 0xffff); - if ((((crc>>8) & 0xff) == data_buff[15]) && ((crc & 0xff) == data_buff[14])) { + if ((HIGHBYTE(crc) == data_buff[15]) && (LOWBYTE(crc) == data_buff[14])) { // status check if (data_buff[13] & 0x02) { // NTRK invalid_count++; } else { // TRK - reading_cm = data_buff[4] * 256 + data_buff[5]; - if (reading_cm < distance_cm_max) { - reading_m = reading_cm * 0.01f; - } else { - reading_m = distance_cm_max * 0.01f; - } + reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f; sum += reading_m; valid_count++; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h index 03115917a5c005..91b2c6a1f13ada 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -1,10 +1,12 @@ #pragma once -#include "AP_RangeFinder.h" -#include "AP_RangeFinder_Backend_Serial.h" +#include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_JRE_SERIAL_ENABLED +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + #define DATA_LENGTH 16 class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial