From 9e27b93538e8b6078db76b922525f84620f0e579 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Aug 2018 15:11:09 +0900 Subject: [PATCH] AP_RangeFinder: move some backend implementations to cpp file --- .../AP_RangeFinder/RangeFinder_Backend.cpp | 21 +++++++++++++++++++ .../AP_RangeFinder/RangeFinder_Backend.h | 20 +++--------------- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index fa9833f086..2ad29b9849 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -30,6 +30,27 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_ _sem = hal.util->new_semaphore(); } +MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const { + if (state.type == RangeFinder::RangeFinder_TYPE_NONE) { + return MAV_DISTANCE_SENSOR_UNKNOWN; + } + return _get_mav_distance_sensor_type(); +} + +RangeFinder::RangeFinder_Status AP_RangeFinder_Backend::status() const { + if (state.type == RangeFinder::RangeFinder_TYPE_NONE) { + // turned off at runtime? + return RangeFinder::RangeFinder_NotConnected; + } + return state.status; +} + +// true if sensor is returning data +bool AP_RangeFinder_Backend::has_data() const { + return ((state.status != RangeFinder::RangeFinder_NotConnected) && + (state.status != RangeFinder::RangeFinder_NoData)); +} + // update status based on distance measurement void AP_RangeFinder_Backend::update_status() { diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h index b04b6853dd..32da7e8f49 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h @@ -41,26 +41,12 @@ public: int16_t max_distance_cm() const { return state.max_distance_cm; } int16_t min_distance_cm() const { return state.min_distance_cm; } int16_t ground_clearance_cm() const { return state.ground_clearance_cm; } - MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const { - if (state.type == RangeFinder::RangeFinder_TYPE_NONE) { - return MAV_DISTANCE_SENSOR_UNKNOWN; - } - return _get_mav_distance_sensor_type(); - } - RangeFinder::RangeFinder_Status status() const { - if (state.type == RangeFinder::RangeFinder_TYPE_NONE) { - // turned off at runtime? - return RangeFinder::RangeFinder_NotConnected; - } - return state.status; - } + MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; + RangeFinder::RangeFinder_Status status() const; RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)state.type.get(); } // true if sensor is returning data - bool has_data() const { - return ((state.status != RangeFinder::RangeFinder_NotConnected) && - (state.status != RangeFinder::RangeFinder_NoData)); - } + bool has_data() const; // returns count of consecutive good readings uint8_t range_valid_count() const { return state.range_valid_count; }