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             :    This program is distributed in the hope that it will be useful,
       7             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       8             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       9             :    GNU General Public License for more details.
      10             :    You should have received a copy of the GNU General Public License
      11             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      12             :  */
      13             : /*
      14             :   Base class for simulator for the TeraRanger Serial RangeFinders
      15             : */
      16             : 
      17             : #include "SIM_RF_TeraRanger_Serial.h"
      18             : 
      19             : using namespace SITL;
      20             : 
      21         354 : uint32_t RF_TeraRanger_Serial::packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen)
      22             : {
      23         354 :     const uint16_t alt_mm = alt_m * 1000;
      24         354 :     buffer[0] = 0x54; //header byte
      25         354 :     buffer[1] = alt_mm >> 8; //MSB mm
      26         354 :     buffer[2] = alt_mm & 0xff; //LSB mm
      27             : 
      28         354 :     if (alt_m > 30) {
      29           0 :        buffer[3] = 0xC4; //full strength, out of range, no overtemp
      30             :     }
      31             :     else {
      32         354 :        buffer[3] = 0xC0; //full strength, no reading error, no overtemp
      33             :     }
      34             :     
      35             :     // calculate CRC8:
      36         354 :     buffer[4] = crc_crc8(buffer,4);;
      37             : 
      38         354 :     return 5;
      39             : }
      40             :