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_MSP.h"
      17             : #include <AP_HAL/AP_HAL.h>
      18             : 
      19             : #if HAL_MSP_RANGEFINDER_ENABLED
      20             : 
      21             : extern const AP_HAL::HAL& hal;
      22             : 
      23             : /*
      24             :    The constructor also initialises the rangefinder. Note that this
      25             :    constructor is not called until detect() returns true, so we
      26             :    already know that we should setup the rangefinder
      27             : */
      28           0 : AP_RangeFinder_MSP::AP_RangeFinder_MSP(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
      29           0 :     AP_RangeFinder_Backend(_state, _params)
      30             : {
      31           0 :     state.last_reading_ms = AP_HAL::millis();
      32           0 :     distance_cm = 0;
      33           0 : }
      34             : 
      35             : /*
      36             :    detect if a MSP rangefinder is connected. We'll detect by
      37             :    checking a parameter.
      38             : */
      39           0 : bool AP_RangeFinder_MSP::detect()
      40             : {
      41             :     // Assume that if the user set the RANGEFINDER_TYPE parameter to MSP,
      42             :     // there is an attached MSP rangefinder
      43           0 :     return true;
      44             : }
      45             : 
      46             : /*
      47             :    Set the distance based on a MSP message
      48             : */
      49           0 : void AP_RangeFinder_MSP::handle_msp(const MSP::msp_rangefinder_data_message_t &pkt)
      50             : {
      51           0 :     state.last_reading_ms = AP_HAL::millis();
      52           0 :     distance_cm = pkt.distance_mm / 10;
      53           0 : }
      54             : 
      55             : /*
      56             :    update the state of the sensor
      57             : */
      58           0 : void AP_RangeFinder_MSP::update(void)
      59             : {
      60             :     //Time out on incoming data; if we don't get new
      61             :     //data in 500ms, dump it
      62           0 :     if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MSP_TIMEOUT_MS) {
      63           0 :         set_status(RangeFinder::Status::NoData);
      64           0 :         state.distance_m = 0.0f;
      65             :     } else {
      66           0 :         state.distance_m = distance_cm * 0.01f;
      67           0 :         update_status();
      68             :     }
      69           0 : }
      70             : 
      71             : #endif //HAL_MSP_RANGEFINDER_ENABLED
      72             :