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_MAVLink.h"
      17             : 
      18             : #if AP_RANGEFINDER_MAVLINK_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : 
      22             : /*
      23             :    Set the distance based on a MAVLINK message
      24             : */
      25         422 : void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
      26             : {
      27             :     mavlink_distance_sensor_t packet;
      28         422 :     mavlink_msg_distance_sensor_decode(&msg, &packet);
      29             : 
      30             :     // only accept distances for the configured orientation and sensor ID
      31         844 :     if (packet.orientation == orientation() &&
      32         422 :         (params.address == 0 || params.address == packet.id)) {
      33         402 :         state.last_reading_ms = AP_HAL::millis();
      34         402 :         distance = packet.current_distance * 0.01;
      35         402 :         _max_distance = packet.max_distance * 0.01;
      36         402 :         _min_distance = packet.min_distance * 0.01;
      37         402 :         sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
      38         402 :         signal_quality = packet.signal_quality;
      39         402 :         if (signal_quality == 0) {
      40             :             // MAVLink's 0 means invalid/unset, so we map it to -1
      41         402 :             signal_quality = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
      42           0 :         } else if (signal_quality == 1) {
      43             :             // Map 1 to 0 as that is what ardupilot uses as the worst signal quality
      44           0 :             signal_quality = RangeFinder::SIGNAL_QUALITY_MIN;
      45             :         }
      46             :     }
      47         422 : }
      48             : 
      49       44408 : float AP_RangeFinder_MAVLink::max_distance() const
      50             : {
      51       44408 :     const auto baseclass_max_distance = AP_RangeFinder_Backend::max_distance();
      52             : 
      53       44408 :     if (is_zero(_max_distance) && is_zero(_min_distance)) {
      54             :         // we assume if both of these are zero that we ignore both
      55        5368 :         return baseclass_max_distance;
      56             :     }
      57             : 
      58             :     // return the smaller of the base class's distance and what we
      59             :     // receive from the network:
      60       39040 :     return MIN(baseclass_max_distance, _max_distance);
      61             : }
      62        2175 : float AP_RangeFinder_MAVLink::min_distance() const
      63             : {
      64        2175 :     const auto baseclass_min_distance = AP_RangeFinder_Backend::min_distance();
      65             : 
      66        2175 :     if (is_zero(_max_distance) && is_zero(_min_distance)) {
      67             :         // we assume if both of these are zero that we ignore both
      68           0 :         return baseclass_min_distance;
      69             :     }
      70             : 
      71             :     // return the larger of the base class's distance and what we
      72             :     // receive from the network:
      73        2175 :     return MAX(baseclass_min_distance, _min_distance);
      74             : }
      75             : 
      76             : /*
      77             :    update the state of the sensor
      78             : */
      79        2505 : void AP_RangeFinder_MAVLink::update(void)
      80             : {
      81             :     //Time out on incoming data; if we don't get new
      82             :     //data in 500ms, dump it
      83        2505 :     if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
      84         796 :         set_status(RangeFinder::Status::NoData);
      85         796 :         state.distance_m = 0.0f;
      86         796 :         state.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
      87             :     } else {
      88        1709 :         state.distance_m = distance;
      89        1709 :         state.signal_quality_pct = signal_quality;
      90        1709 :         update_status();
      91             :     }
      92        2505 : }
      93             : 
      94             : #endif