Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_RangeFinder_GYUS42v2.h"
      17             : 
      18             : #if AP_RANGEFINDER_GYUS42V2_ENABLED
      19             : 
      20           0 : bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start)
      21             : {
      22           0 :     for (uint8_t i=start; i<buffer_used; i++) {
      23           0 :         if (buffer[i] == 0x5A) {
      24           0 :             memmove(&buffer[0], &buffer[i], buffer_used-i);
      25           0 :             buffer_used -= i;
      26           0 :             return true;
      27             :         }
      28             :     }
      29             :     // header byte not in buffer
      30           0 :     buffer_used = 0;
      31           0 :     return false;
      32             : }
      33             : 
      34             : // get_reading - read a value from the sensor
      35        1352 : bool AP_RangeFinder_GYUS42v2::get_reading(float &reading_m)
      36             : {
      37        1352 :     if (uart == nullptr) {
      38           0 :         return false;
      39             :     }
      40             : 
      41        1352 :     const uint8_t num_read = uart->read(&buffer[buffer_used], ARRAY_SIZE(buffer)-buffer_used);
      42        1352 :     buffer_used += num_read;
      43             : 
      44        1352 :     if (buffer_used == 0) {
      45         665 :         return false;
      46             :     }
      47             : 
      48         687 :     if (buffer[0] != 0x5A &&
      49           0 :         !find_signature_in_buffer(1)) {
      50           0 :         return false;
      51             :     }
      52             : 
      53         687 :     if (buffer_used < ARRAY_SIZE(buffer)) {
      54           0 :         return false;
      55             :     }
      56             : 
      57         687 :     uint8_t sum = 0;
      58        4809 :     for (uint8_t i=0; i<6; i++) {
      59        4122 :         sum += buffer[i];
      60             :     }
      61         687 :     if (buffer[6] != sum) {
      62           0 :         find_signature_in_buffer(1);
      63           0 :         return false;
      64             :     }
      65             : 
      66         687 :     reading_m = (buffer[4] << 8 | buffer[5]) * 0.01f;
      67         687 :     buffer_used = 0;
      68             : 
      69         687 :     return true;
      70             : }
      71             : 
      72             : #endif  // AP_RANGEFINDER_GYUS42V2_ENABLED