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_config.h"
      17             : 
      18             : #if AP_RANGEFINDER_LUA_ENABLED
      19             : 
      20             : #include "AP_RangeFinder_Lua.h"
      21             : #include <AP_HAL/AP_HAL.h>
      22             : 
      23             : // constructor
      24           5 : AP_RangeFinder_Lua::AP_RangeFinder_Lua(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
      25           5 :     AP_RangeFinder_Backend(_state, _params)
      26             : {
      27           5 : }
      28             : 
      29             : 
      30             : // Process range finder data from a lua driver. The state structure needs to be completely
      31             : // filled in by the lua script. The data passed to this method is copied to a pending_state
      32             : // structure. The update() method periodically copies data from pending_state to state. The get_state()
      33             : // method returns data from state.
      34       17063 : bool AP_RangeFinder_Lua::handle_script_msg(const RangeFinder::RangeFinder_State &state_arg)
      35             : {
      36       17063 :     WITH_SEMAPHORE(_sem);
      37       17063 :     _state_pending = state_arg;
      38       34126 :     return true;
      39       17063 : }
      40             : 
      41             : // Process range finder data from a lua driver - legacy interface. This method takes
      42             : // a distance measurement and fills in the pending state structure. In this legacy mode
      43             : // the lua script only passes in a distance measurement and does not manage the rest
      44             : // of the fields in the state structure.
      45           3 : bool AP_RangeFinder_Lua::handle_script_msg(float dist_m) {
      46             : 
      47           3 :     const uint32_t now = AP_HAL::millis();
      48             : 
      49           3 :     WITH_SEMAPHORE(_sem);
      50             : 
      51           3 :     _state_pending.last_reading_ms = now;
      52           3 :     _state_pending.distance_m = dist_m;
      53           3 :     _state_pending.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
      54           3 :     _state_pending.voltage_mv = 0;
      55           3 :     update_status(_state_pending);
      56             : 
      57           6 :     return true;
      58           3 : }
      59             : 
      60             : // Update the state of the sensor
      61       17470 : void AP_RangeFinder_Lua::update(void)
      62             : {
      63       17470 :     WITH_SEMAPHORE(_sem);
      64             : 
      65             :     // Time out on incoming data
      66       34938 :     if (_state_pending.status != RangeFinder::Status::NotConnected &&
      67       17468 :             AP_HAL::millis() - _state_pending.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) {
      68         139 :         set_status(_state_pending, RangeFinder::Status::NoData);
      69             :     }
      70       17470 :     state = _state_pending;
      71       17470 : }
      72             : 
      73             : #endif  // AP_RANGEFINDER_LUA_ENABLED