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_Proximity_config.h"
      17             : 
      18             : #if AP_PROXIMITY_RANGEFINDER_ENABLED
      19             : 
      20             : #include "AP_Proximity_RangeFinder.h"
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <ctype.h>
      24             : #include <stdio.h>
      25             : #include <AP_RangeFinder/AP_RangeFinder.h>
      26             : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
      27             : 
      28             : // update the state of the sensor
      29           0 : void AP_Proximity_RangeFinder::update(void)
      30             : {
      31             :     // exit immediately if no rangefinder object
      32           0 :     const RangeFinder *rngfnd = AP::rangefinder();
      33           0 :     if (rngfnd == nullptr) {
      34           0 :         set_status(AP_Proximity::Status::NoData);
      35           0 :         return;
      36             :     }
      37             : 
      38           0 :     uint32_t now = AP_HAL::millis();
      39             : 
      40             :     // look through all rangefinders
      41           0 :     for (uint8_t i=0; i < rngfnd->num_sensors(); i++) {
      42           0 :         AP_RangeFinder_Backend *sensor = rngfnd->get_backend(i);
      43           0 :         if (sensor == nullptr) {
      44           0 :             continue;
      45             :         }
      46           0 :         if (sensor->has_data()) {
      47           0 :             const bool reading_is_good = (sensor->status() == RangeFinder::Status::Good);
      48             :             // check for horizontal range finders
      49           0 :             if (sensor->orientation() <= ROTATION_YAW_315) {
      50           0 :                 const uint8_t sector = (uint8_t)sensor->orientation();
      51           0 :                 const float angle = sector * 45;
      52           0 :                 const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle);
      53             :                 // distance in meters
      54           0 :                 const float distance = sensor->distance();
      55           0 :                 _distance_min = sensor->min_distance();
      56           0 :                 _distance_max = sensor->max_distance();
      57           0 :                 if (reading_is_good && (distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) {
      58           0 :                     frontend.boundary.set_face_attributes(face, angle, distance, state.instance);
      59             :                     // update OA database
      60           0 :                     database_push(angle, distance);
      61             :                 } else {
      62           0 :                     frontend.boundary.reset_face(face, state.instance);
      63             :                 }
      64           0 :                 _last_update_ms = now;
      65             :             }
      66             :             // check upward facing range finder
      67           0 :             if (sensor->orientation() == ROTATION_PITCH_90) {
      68           0 :                 const float distance_upward = sensor->distance();
      69           0 :                 const float up_distance_min = sensor->min_distance();
      70           0 :                 const float up_distance_max = sensor->max_distance();
      71           0 :                 if (reading_is_good && (distance_upward >= up_distance_min) && (distance_upward <= up_distance_max)) {
      72           0 :                     _distance_upward = distance_upward;
      73             :                 } else {
      74           0 :                     _distance_upward = -1.0; // mark an valid reading
      75             :                 }
      76           0 :                 _last_upward_update_ms = now;
      77             :             }
      78             :         }
      79             :     }
      80             : 
      81             :     // check for timeout and set health status
      82           0 :     if ((_last_update_ms == 0 || (now - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) &&
      83           0 :         (_last_upward_update_ms == 0 || (now - _last_upward_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS))) {
      84           0 :         set_status(AP_Proximity::Status::NoData);
      85             :     } else {
      86           0 :         set_status(AP_Proximity::Status::Good);
      87             :     }
      88             : }
      89             : 
      90             : // get distance upwards in meters. returns true on success
      91           0 : bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const
      92             : {
      93           0 :     if ((AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_RANGEFIDER_TIMEOUT_MS) &&
      94           0 :         is_positive(_distance_upward)) {
      95           0 :         distance = _distance_upward;
      96           0 :         return true;
      97             :     }
      98           0 :     return false;
      99             : }
     100             : 
     101             : #endif // AP_PROXIMITY_RANGEFINDER_ENABLED