Skip to content

Commit

Permalink
This commit is a pre-check before updating the master_JRE branch 2
Browse files Browse the repository at this point in the history
  • Loading branch information
@jfbblue0922
jfbblue0922 committed Oct 13, 2023
1 parent 7356b7c commit 21d9661
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 17 deletions.
40 changes: 25 additions & 15 deletions libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,10 @@
#include "AP_RangeFinder_JRE_Serial.h"
#include <AP_Math/AP_Math.h>

#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
Expand All @@ -41,36 +43,43 @@ 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
for (; read_buff_idx < num_read; read_buff_idx++) {
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;
Expand All @@ -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];
}
}
}
Expand Down
60 changes: 58 additions & 2 deletions libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
};
Expand Down

0 comments on commit 21d9661

Please sign in to comment.