/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ #include "AP_RangeFinder_GYUS42v2.h" #if AP_RANGEFINDER_GYUS42V2_ENABLED bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start) { for (uint8_t i=start; i<buffer_used; i++) { if (buffer[i] == 0x5A) { memmove(&buffer[0], &buffer[i], buffer_used-i); buffer_used -= i; return true; } } // header byte not in buffer buffer_used = 0; return false; } // get_reading - read a value from the sensor bool AP_RangeFinder_GYUS42v2::get_reading(float &reading_m) { if (uart == nullptr) { return false; } const uint8_t num_read = uart->read(&buffer[buffer_used], ARRAY_SIZE(buffer)-buffer_used); buffer_used += num_read; if (buffer_used == 0) { return false; } if (buffer[0] != 0x5A && !find_signature_in_buffer(1)) { return false; } if (buffer_used < ARRAY_SIZE(buffer)) { return false; } uint8_t sum = 0; for (uint8_t i=0; i<6; i++) { sum += buffer[i]; } if (buffer[6] != sum) { find_signature_in_buffer(1); return false; } reading_m = (buffer[4] << 8 | buffer[5]) * 0.01f; buffer_used = 0; return true; } #endif // AP_RANGEFINDER_GYUS42V2_ENABLED