From 040ef41b58d888339051cf761ff1111965dbca7d Mon Sep 17 00:00:00 2001 From: jfbblue0922 Date: Wed, 26 Jul 2023 21:22:54 +0900 Subject: [PATCH] AP_RangeFinder: update serial driver for JRE --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 4 +- .../AP_RangeFinder_JRE_Serial.cpp | 66 ++++++++----------- .../AP_RangeFinder_JRE_Serial.h | 20 +++++- 3 files changed, 48 insertions(+), 42 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index d53e161a943c40..2222fc0a818532 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -542,11 +542,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) serial_create_fn = AP_RangeFinder_NoopLoop::create; #endif break; - case Type::JRE_Serial: #if AP_RANGEFINDER_JRE_SERIAL_ENABLED + case Type::JRE_Serial: serial_create_fn = AP_RangeFinder_JRE_Serial::create; -#endif break; +#endif case Type::NONE: break; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index 2125678a602796..df742fda80ede1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -13,15 +13,18 @@ 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 #define DIST_MAX_CM 50000 +#define OUT_OF_RANGE_ADD_CM 100 bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) { @@ -29,13 +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 count = 0; - float reading_cm; + uint16_t valid_count = 0; // number of valid readings + uint16_t invalid_count = 0; // number of invalid readings + float sum = 0; // max distance the sensor can reliably measure - read from parameters const int16_t distance_cm_max = max_distance_cm(); @@ -46,25 +46,12 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) if (data_buff_idx == 0) { // header first byte check // header data search - for (; read_buff_idx= num_read - 1) { // read end byte - if (count > 0) { - return true; - } - return false; // next packet to header second byte - } else { - if (read_buff[read_buff_idx + 1] == FRAME_HEADER_2) { - data_buff[1] = FRAME_HEADER_2; - data_buff_idx = 2; // next data_buff - read_buff_idx += 2; // next read_buff - break; // next data set - } else { - data_buff_idx = 0; // data index clear - } - } + read_buff_idx++; // next read_buff + break; } } @@ -75,27 +62,21 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) } else { data_buff_idx = 0; // data index clear } - read_buff_idx++; // next read_buff + read_buff_idx++; // next read_buff } else { // data set 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 - no_signal = true; - reading_m = MIN(MAX(DIST_MAX_CM, distance_cm_max), UINT16_MAX) * 0.01f; - } else { // UPDATE DATA - no_signal = false; - 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; - } + invalid_count++; + } else { // TRK + reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f; + sum += reading_m; + valid_count++; } - count++; } data_buff_idx = 0; // data index clear } else { @@ -104,9 +85,18 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) } } - if (count > 0) { + if (valid_count > 0) { + no_signal = false; + reading_m = sum / valid_count; return true; } + + if (invalid_count > 0) { + no_signal = true; + reading_m = MIN(MAX(DIST_MAX_CM, distance_cm_max + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; + return true; + } + return false; } #endif // AP_RANGEFINDER_JRE_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h index 03115917a5c005..16ce9f70824724 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -1,10 +1,26 @@ #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" + +/* +The data received from the radio altimeter is as follows. +The received data frame format is 16 bytes fixed and is stored in 'read_buff'. +|--------------------------------------------------------------------------------------------------------------------------------------------------| +| BYTE | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | +|------|-------------------------------------------------------------------------------------------------------------------------------------------| +| NAME | header | header | version | frame | altitude | altitude | reserved | FFT value | FFT value | status | status | CRC | CRC | +| | (H) | (L) | | count | (H) | (L) | | (H) | (L) | (H) | (L) | (L) | (H) | +|------|-------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | unsigned | unsigned | | unsigned | BIT 2to15: reserved | CRC result | +| | 'R' | 'A' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | BIT 1: TRK / NTRK | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | BIT 0: GAIN | 0 to 13 | +|--------------------------------------------------------------------------------------------------------------------------------------------------| +*/ #define DATA_LENGTH 16 class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial