From 21d966193c410ab0f7a4314caf6c05e6d9077401 Mon Sep 17 00:00:00 2001 From: jfbblue0922 Date: Tue, 10 Oct 2023 15:03:49 +0900 Subject: [PATCH] This commit is a pre-check before updating the master_JRE branch 2 --- .../AP_RangeFinder_JRE_Serial.cpp | 40 ++++++++----- .../AP_RangeFinder_JRE_Serial.h | 60 ++++++++++++++++++- 2 files changed, 83 insertions(+), 17 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index df742fda80ede1..9c3ed52199c890 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -20,8 +20,10 @@ #include "AP_RangeFinder_JRE_Serial.h" #include -#define FRAME_HEADER_1 'R' // 0x52 -#define FRAME_HEADER_2 'A' // 0x41 +#define FRAME_HEADER_1 'R' // 0x52 +#define FRAME_HEADER_2_A 'A' // 0x41 +#define FRAME_HEADER_2_B 'B' // 0x42 +#define FRAME_HEADER_2_C 'C' // 0x43 #define DIST_MAX_CM 50000 #define OUT_OF_RANGE_ADD_CM 100 @@ -41,8 +43,7 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) // buffer read const ssize_t num_read = uart->read(read_buff, ARRAY_SIZE(read_buff)); - read_buff_idx = 0; // read_buff start data index - while (read_buff_idx < num_read) { + for (read_buff_idx = 0; read_buff_idx < num_read; read_buff_idx++) { if (data_buff_idx == 0) { // header first byte check // header data search @@ -50,27 +51,35 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) if (read_buff[read_buff_idx] == FRAME_HEADER_1) { data_buff[0] = FRAME_HEADER_1; data_buff_idx = 1; // next data_buff - read_buff_idx++; // next read_buff break; } } } else if (data_buff_idx == 1) { // header second byte check - if (read_buff[read_buff_idx] == FRAME_HEADER_2) { - data_buff[1] = FRAME_HEADER_2; - data_buff_idx = 2; // next data_buff - } else { + data_buff[1] = read_buff[read_buff_idx]; + data_buff_idx = 2; // next data_buff + switch (read_buff[read_buff_idx]) { + case FRAME_HEADER_2_A: + data_buff_len = 16; + break; + case FRAME_HEADER_2_B: + data_buff_len = 32; + break; + case FRAME_HEADER_2_C: + data_buff_len = 48; + break; + default: data_buff_idx = 0; // data index clear + break; } - read_buff_idx++; // next read_buff } else { // data set - if (data_buff_idx >= ARRAY_SIZE(data_buff)) { // 1 data set complete + if (data_buff_idx >= data_buff_len) { // 1 data set complete // crc check - uint16_t crc = crc16_ccitt_r(data_buff, ARRAY_SIZE(data_buff) - 2, 0xffff, 0xffff); - if ((HIGHBYTE(crc) == data_buff[15]) && (LOWBYTE(crc) == data_buff[14])) { + uint16_t crc = crc16_ccitt_r(data_buff, data_buff_len - 2, 0xffff, 0xffff); + if ((HIGHBYTE(crc) == data_buff[data_buff_len-1]) && (LOWBYTE(crc) == data_buff[data_buff_len-2])) { // status check - if (data_buff[13] & 0x02) { // NTRK + if (data_buff[data_buff_len-3] & 0x02) { // NTRK invalid_count++; } else { // TRK reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f; @@ -79,8 +88,9 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) } } data_buff_idx = 0; // data index clear + read_buff_idx--; // exclude current index } else { - data_buff[data_buff_idx++] = read_buff[read_buff_idx++]; + data_buff[data_buff_idx++] = read_buff[read_buff_idx]; } } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h index 91b2c6a1f13ada..4c7dbea836d478 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -7,6 +7,61 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +/* +The data received from the radio altimeter is as follows. +The format of the received data frame varies depending on the mode, and is stored in "read_buff" with a fixed value of 16 bytes, 32 bytes, or 48 bytes. + +[1] +Measurement mode: 1 data mode +Packet length: 16 bytes +Altitude data used: 4,5 bytes +|----------------------------------------------------------------------------------------------------------------------------------------------------| +| 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) | +|------|---------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | | | | | BIT 15to8: reserved | | +| | | | | unsigned | unsigned | | unsigned | BIT 7to4: GAIN detail | CRC result | +| DATA | 'R' | 'A' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | BIT 3-2: reserved | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | BIT 1: TRK / NTRK | 0 to 13 | +| | | | | | | | | BIT 0: GAIN LOW/HIGH | | +|----------------------------------------------------------------------------------------------------------------------------------------------------| + +[2] +Measurement mode: 3 data mode +Packet length: 32 bytes +Altitude data used: 4,5 bytes +|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| BYTE | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14| 15| 16| 17| 18 | 19 | 20 | 21 | 22| 23| 24| 25| 26 | 27 | 28 | 29 | 30 | 31 | +|------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| NAME | header | header | version | frame | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | status | status | CRC | CRC | +| | (H) | (L) | | count | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | (L) | (H) | +|------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | | | | | | | | | | | BIT 15to8: reserved | | +| | | | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | BIT 7to4: GAIN detail | CRC result | +| DATA | 'R' | 'B' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | BIT 3-2: reserved | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | BIT 1: TRK / NTRK | 0 to 13 | +| | | | | | | | | | | | | | | BIT 0: GAIN LOW/HIGH | | +|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| + +[3] +Measurement mode: 5 data mode +Packet length: 48 bytes +Altitude data used: 4,5 bytes +|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| BYTE | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14| 15| 16| 17| 18 | 19 | 20 | 21 | 22| 23| 24| 25| 26 | 27 | 28 | 29 | 30| 31| 32| 33| 34 | 35 | 36 | 37 | 38| 39| 40| 41| 42 | 43 | 44 | 45 | 46 | 47 | +|------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| NAME | header | header | version | frame | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | status | status | CRC | CRC | +| | (H) | (L) | | count | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | (L) | (H) | +|------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | | | | | | | | | | | | | | | | | BIT 15to8: reserved | | +| | | | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | BIT 7to4: GAIN detail | CRC result | +| DATA | 'R' | 'C' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | BIT 3-2: reserved | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | BIT 1: TRK / NTRK | 0 to 13 | +| | | | | | | | | | | | | | | | | | | | | BIT 0: GAIN LOW/HIGH | | +|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +*/ #define DATA_LENGTH 16 class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial @@ -41,10 +96,11 @@ class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial bool get_reading(float &reading_m) override; uint8_t read_num; - uint8_t read_buff[DATA_LENGTH * 8]; + uint8_t read_buff[DATA_LENGTH * 10]; uint8_t read_buff_idx; - uint8_t data_buff[DATA_LENGTH]; + uint8_t data_buff[DATA_LENGTH * 3]; // maximum frame length uint8_t data_buff_idx; + uint8_t data_buff_len; bool no_signal; };