Skip to content

Commit

Permalink
AP_RangeFinder: JRE rename data_buff_ofs
Browse files Browse the repository at this point in the history
  • Loading branch information
rmackay9 committed Nov 15, 2023
1 parent 6bd7024 commit da6f31d
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 9 deletions.
16 changes: 8 additions & 8 deletions libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,16 +31,16 @@
void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos)
{
uint8_t i;
for (i=search_start_pos; i<data_buff_idx; i++) {
for (i=search_start_pos; i<data_buff_ofs; i++) {
if (data_buff[i] == FRAME_HEADER_1) {
break;
}
}
if (i == 0) {
return;
}
memmove(data_buff, &data_buff[i], data_buff_idx-i);
data_buff_idx = data_buff_idx - i;
memmove(data_buff, &data_buff[i], data_buff_ofs-i);
data_buff_ofs = data_buff_ofs - i;
}

bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
Expand All @@ -61,8 +61,8 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)

while (bytes_available > 0) {
// fill buffer
const auto num_bytes_to_read = MIN(bytes_available, ARRAY_SIZE(data_buff) - data_buff_idx);
const auto num_bytes_read = uart->read(&data_buff[data_buff_idx], num_bytes_to_read);
const auto num_bytes_to_read = MIN(bytes_available, ARRAY_SIZE(data_buff) - data_buff_ofs);
const auto num_bytes_read = uart->read(&data_buff[data_buff_ofs], num_bytes_to_read);
if (num_bytes_read == 0) {
break;
}
Expand All @@ -71,13 +71,13 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
break;
}
bytes_available -= num_bytes_read;
data_buff_idx += num_bytes_read;
data_buff_ofs += num_bytes_read;

// move header frame header in buffer
move_preamble_in_buffer(0);

// ensure we have a packet type:
if (data_buff_idx < 2) {
if (data_buff_ofs < 2) {
continue;
}

Expand All @@ -99,7 +99,7 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
}

// check there are enough bytes for message type
if (data_buff_idx < packet_length) {
if (data_buff_ofs < packet_length) {
continue;
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial
void move_preamble_in_buffer(uint8_t search_start_pos);

uint8_t data_buff[48 * 3]; // 48 is longest possible packet
uint8_t data_buff_idx;
uint8_t data_buff_ofs; // index where next item will be added in data_buff

bool no_signal; // true if the latest read attempt found no valid distances
};
Expand Down

0 comments on commit da6f31d

Please sign in to comment.