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             : #pragma once
      16             : 
      17             : #include "AP_Proximity.h"
      18             : 
      19             : #if HAL_PROXIMITY_ENABLED
      20             : #include <AP_Common/AP_Common.h>
      21             : #include <AP_HAL/Semaphores.h>
      22             : 
      23             : class AP_Proximity_Backend
      24             : {
      25             : public:
      26             :     // constructor. This incorporates initialisation as well.
      27             :         AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params &_params);
      28             : 
      29             :     // we declare a virtual destructor so that Proximity drivers can
      30             :     // override with a custom destructor if need be
      31           0 :     virtual ~AP_Proximity_Backend(void) {}
      32             : 
      33             :     // update the state structure
      34             :     virtual void update() = 0;
      35             : 
      36             :     // get maximum and minimum distances (in meters) of sensor
      37             :     virtual float distance_max_m() const = 0;
      38             :     virtual float distance_min_m() const = 0;
      39             : 
      40             :     // get distance upwards in meters. returns true on success
      41        2851 :     virtual bool get_upward_distance(float &distance) const { return false; }
      42             : 
      43             :     // handle mavlink messages
      44           0 :     virtual void handle_msg(const mavlink_message_t &msg) {}
      45             : 
      46             : #if AP_SCRIPTING_ENABLED
      47             :     // handle scripting obstacle messages
      48           0 :     virtual bool set_distance_min_max(float min, float max) { return false; }
      49             :     // this is in body frame
      50           0 :     virtual bool handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary) { return false; }
      51           0 :     virtual bool handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary) { return false; }
      52           0 :     virtual bool update_virtual_boundary() { return false; }
      53             : #endif
      54             : 
      55             :     // return the type of sensor
      56           0 :     AP_Proximity::Type type() const { return (AP_Proximity::Type)params.type.get(); }
      57             : 
      58             : protected:
      59             : 
      60             :     // set status and update valid_count
      61             :     void set_status(AP_Proximity::Status status);
      62             : 
      63             :     // correct an angle (in degrees) based on the orientation and yaw correction parameters
      64             :     float correct_angle_for_orientation(float angle_degrees) const;
      65             : 
      66             :     // check if a reading should be ignored because it falls into an ignore area (check_for_ign_area should be sent as false if this check is not needed)
      67             :     // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
      68             :     // yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
      69             :     // Also checks if obstacle is near land or out of range
      70             :     // angles should be in degrees and in the range of 0 to 360, distance should be in meteres
      71             :     bool ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area = true) const;
      72      428928 :     bool ignore_reading(float yaw, float distance_m, bool check_for_ign_area = true) const { return ignore_reading(0.0f, yaw, distance_m, check_for_ign_area); }
      73             : 
      74             :     // database helpers. All angles are in degrees
      75             :     static bool database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned);
      76             :     // Note: "angle" refers to yaw (in body frame) towards the obstacle
      77             :     static void database_push(float angle, float pitch, float distance);
      78      334514 :     static void database_push(float angle, float distance) {
      79      334514 :         database_push(angle, 0.0f, distance);
      80      334514 :     }
      81             : 
      82           0 :     static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned) {
      83           0 :         database_push(angle, 0.0f, distance, timestamp_ms, current_pos, body_to_ned);
      84           0 :     };
      85             :     static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);
      86             : 
      87             :     // semaphore for access to shared frontend data
      88             :     HAL_Semaphore _sem;
      89             : 
      90             :     AP_Proximity::Type _backend_type;
      91             : 
      92             :     AP_Proximity &frontend;
      93             :     AP_Proximity::Proximity_State &state;   // reference to this instances state
      94             :     AP_Proximity_Params &params;            // parameters for this backend
      95             : };
      96             : 
      97             : #endif // HAL_PROXIMITY_ENABLED