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             :   Base class for serial rangefinders
      17             : */
      18             : 
      19             : #include "SIM_SerialRangeFinder.h"
      20             : 
      21             : using namespace SITL;
      22             : 
      23     2817506 : void SerialRangeFinder::update(float range)
      24             : {
      25             :     // just send a chunk of data at 5Hz:
      26     2817506 :     const uint32_t now = AP_HAL::millis();
      27     2817506 :     if (now - last_sent_ms < reading_interval_ms()) {
      28     2701828 :         return;
      29             :     }
      30      115678 :     last_sent_ms = now;
      31             : 
      32             :     uint8_t data[255];
      33      115678 :     const uint32_t packetlen = packet_for_alt(range,
      34             :                                               data,
      35             :                                               ARRAY_SIZE(data));
      36             : 
      37      115678 :     write_to_autopilot((char*)data, packetlen);
      38             : 
      39      115678 :     if (has_temperature()) {
      40        1583 :         send_temperature();
      41             :     }
      42             : 
      43             : }
      44             : 
      45        1583 : void SerialRangeFinder::send_temperature()
      46             : {
      47             :     // Use the simple underwater model to get temperature
      48             :     float rho, delta, theta;
      49        1583 :     AP_Baro::SimpleUnderWaterAtmosphere(-0.5 * 0.001, rho, delta, theta); // get simulated temperature for 0.5m depth
      50        1583 :     const float temperature = Aircraft::rand_normal(KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta), 1); // FIXME pick a stddev based on data sheet
      51             : 
      52             :     uint8_t data[255];
      53        1583 :     const uint32_t packetlen = packet_for_temperature(temperature,
      54             :                                                         data,
      55             :                                                         ARRAY_SIZE(data));
      56        1583 :     write_to_autopilot((char*)data, packetlen);
      57        1583 : }