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_config.h"
      18             : 
      19             : #if HAL_PROXIMITY_ENABLED
      20             : 
      21             : #include <AP_Common/AP_Common.h>
      22             : #include <AP_Param/AP_Param.h>
      23             : #include <AP_Math/AP_Math.h>
      24             : #include <GCS_MAVLink/GCS_MAVLink.h>
      25             : #include "AP_Proximity_Params.h"
      26             : #include "AP_Proximity_Boundary_3D.h"
      27             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      28             : 
      29             : #include <AP_HAL/Semaphores.h>
      30             : 
      31             : #ifndef AP_PROXIMITY_MAX_INSTANCES 
      32             :   #define AP_PROXIMITY_MAX_INSTANCES             5   // Maximum number of proximity sensor instances available on this platform
      33             : #endif
      34             : #define PROXIMITY_SENSOR_ID_START 10
      35             : 
      36             : class AP_Proximity_Backend;
      37             : 
      38             : class AP_Proximity
      39             : {
      40             : public:
      41             :     friend class AP_Proximity_Backend;
      42             :     friend class AP_Proximity_DroneCAN;
      43             : 
      44             :     AP_Proximity();
      45             : 
      46             :     /* Do not allow copies */
      47             :     CLASS_NO_COPY(AP_Proximity);
      48             : 
      49             :     // Proximity driver types
      50             :     enum class Type {
      51             :         None    = 0,
      52             :         // 1 was SF40C_v09
      53             : #if AP_PROXIMITY_MAV_ENABLED
      54             :         MAV     = 2,
      55             : #endif
      56             : #if AP_PROXIMITY_TERARANGERTOWER_ENABLED
      57             :         TRTOWER = 3,
      58             : #endif
      59             : #if AP_PROXIMITY_RANGEFINDER_ENABLED
      60             :         RangeFinder = 4,
      61             : #endif
      62             : #if AP_PROXIMITY_RPLIDARA2_ENABLED
      63             :         RPLidarA2 = 5,
      64             : #endif
      65             : #if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
      66             :         TRTOWEREVO = 6,
      67             : #endif
      68             : #if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
      69             :         SF40C = 7,
      70             : #endif
      71             : #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
      72             :         SF45B = 8,
      73             : #endif
      74             : #if AP_PROXIMITY_SITL_ENABLED
      75             :         SITL    = 10,
      76             : #endif
      77             : #if AP_PROXIMITY_AIRSIMSITL_ENABLED
      78             :         AirSimSITL = 12,
      79             : #endif
      80             : #if AP_PROXIMITY_CYGBOT_ENABLED
      81             :         CYGBOT_D1 = 13,
      82             : #endif
      83             : #if AP_PROXIMITY_DRONECAN_ENABLED
      84             :         DroneCAN = 14,
      85             : #endif
      86             : #if AP_PROXIMITY_SCRIPTING_ENABLED
      87             :         Scripting = 15,
      88             : #endif
      89             : #if AP_PROXIMITY_LD06_ENABLED
      90             :         LD06 = 16,
      91             : #endif
      92             : #if AP_PROXIMITY_MR72_ENABLED
      93             :         MR72 = 17,
      94             : #endif
      95             : #if AP_PROXIMITY_HEXSOONRADAR_ENABLED
      96             :         Hexsoon_Radar = 18,
      97             : #endif
      98             :     };
      99             : 
     100             :     enum class Status {
     101             :         NotConnected = 0,
     102             :         NoData,
     103             :         Good
     104             :     };
     105             : 
     106             :     // detect and initialise any available proximity sensors
     107             :     __INITFUNC__ void init();
     108             : 
     109             :     // update state of all proximity sensors. Should be called at high rate from main loop
     110             :     void update();
     111             : 
     112             :     // return the number of proximity sensor backends
     113     1416439 :     uint8_t num_sensors() const { return num_instances; }
     114             : 
     115             :     // return sensor type of a given instance
     116             :     Type get_type(uint8_t instance) const;
     117             : 
     118             :     // return distance filter frequency
     119    15835009 :     float get_filter_freq() const { return _filt_freq; }
     120             : 
     121             :     // return sensor health
     122             :     Status get_instance_status(uint8_t instance) const;
     123             : 
     124             :     // Returns status of first good sensor. If no good sensor found, returns status of last instance sensor 
     125             :     Status get_status() const;
     126             : 
     127             :     // prearm checks
     128             :     bool prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const;
     129             : 
     130             :     // get maximum and minimum distances (in meters)
     131             :     float distance_max_m() const;
     132             :     float distance_min_m() const;
     133             : 
     134             :     //
     135             :     // 3D boundary related methods
     136             :     //
     137             : 
     138             :     // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
     139             :     bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
     140             : 
     141             :     // get total number of obstacles, used in GPS based Simple Avoidance
     142             :     uint8_t get_obstacle_count() const;
     143             : 
     144             :     // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
     145             :     bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const;
     146             : 
     147             :     // returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
     148             :     // returns FLT_MAX if it's an invalid instance.
     149             :     bool closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
     150             : 
     151             :     // get distance and angle to closest object (used for pre-arm check)
     152             :     //   returns true on success, false if no valid readings
     153             :     bool get_closest_object(float& angle_deg, float &distance) const;
     154             : 
     155             :     // get number of objects
     156             :     uint8_t get_object_count() const;
     157             :     bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
     158             : 
     159             :     // get obstacle pitch and angle for a particular obstacle num
     160             :     bool get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch, float &distance) const;
     161             : 
     162             :     //
     163             :     // mavlink related methods
     164             :     //
     165             : 
     166             :     // handle mavlink messages
     167             :     void handle_msg(const mavlink_message_t &msg);
     168             : 
     169             :     // methods for mavlink SYS_STATUS message (send_sys_status)
     170             :     bool sensor_present() const;
     171             :     bool sensor_enabled() const;
     172             :     bool sensor_failed() const;
     173             : 
     174             :     //
     175             :     // support for upwards and downwards facing sensors
     176             :     //
     177             : 
     178             :     // get distance upwards in meters. returns true on success
     179             :     bool get_upward_distance(uint8_t instance, float &distance_m) const;
     180             :     bool get_upward_distance(float &distance_m) const;
     181             : 
     182             :     // set alt as read from downward facing rangefinder. Tilt is already adjusted for
     183             :     void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
     184             : 
     185             :     // method called by vehicle to have AP_Proximity write onboard log messages
     186             :     void log();
     187             : 
     188             :     // The Proximity_State structure is filled in by the backend driver
     189             :     struct Proximity_State {
     190             :         uint8_t instance;   // the instance number of this proximity sensor
     191             :         Status status;      // sensor status
     192             : 
     193             :         const struct AP_Param::GroupInfo *var_info; // stores extra parameter information for the sensor (if it exists)
     194             :     };
     195             : 
     196             :     static const struct AP_Param::GroupInfo *backend_var_info[AP_PROXIMITY_MAX_INSTANCES];
     197             : 
     198             :     // parameter list
     199             :     static const struct AP_Param::GroupInfo var_info[];
     200             : 
     201     5939182 :     static AP_Proximity *get_singleton(void) { return _singleton; };
     202             : 
     203             :     // return backend object for Lua scripting
     204             :     AP_Proximity_Backend *get_backend(uint8_t id) const;
     205             : 
     206             :     // 3D boundary
     207             :     AP_Proximity_Boundary_3D boundary;
     208             : 
     209             :     // Check if Obstacle defined by body-frame yaw and pitch is near ground
     210             :     bool check_obstacle_near_ground(float pitch, float yaw, float distance) const;
     211             : 
     212             :     // get proximity address (for AP_Periph CAN)
     213             :     uint8_t get_address(uint8_t id) const {
     214             :         return id >= AP_PROXIMITY_MAX_INSTANCES? 0 : uint8_t(params[id].address.get());
     215             :     }
     216             : 
     217             : protected:
     218             : 
     219             :     // parameters for backends
     220             :     AP_Proximity_Params params[AP_PROXIMITY_MAX_INSTANCES];
     221             : 
     222             : private:
     223             :     static AP_Proximity *_singleton;
     224             :     Proximity_State state[AP_PROXIMITY_MAX_INSTANCES];
     225             :     AP_Proximity_Backend *drivers[AP_PROXIMITY_MAX_INSTANCES];
     226             :     uint8_t num_instances;
     227             : 
     228             :     // return true if the given instance exists
     229             :     bool valid_instance(uint8_t i) const;
     230             : 
     231             :     // parameters for all instances
     232             :     AP_Int8 _raw_log_enable;                    // enable logging raw distances
     233             :     AP_Int8 _ign_gnd_enable;                    // true if land detection should be enabled
     234             :     AP_Float _filt_freq;                        // cutoff frequency for low pass filter
     235             :     AP_Float _alt_min_m;                        // Minimum altitude -in meters- below which proximity should not work.
     236             : 
     237             :     // get alt from rangefinder in meters. This reading is corrected for vehicle tilt
     238             :     bool get_rangefinder_alt(float &alt_m) const;
     239             : 
     240             :     struct RangeFinderState {
     241             :         bool use;                               // true if enabled
     242             :         bool healthy;                           // true if we can trust the altitude from the rangefinder
     243             :         int16_t alt_cm;                         // tilt compensated altitude (in cm) from rangefinder
     244             :         uint32_t last_downward_update_ms;       // last update ms
     245             :     } _rangefinder_state;
     246             : 
     247             :     HAL_Semaphore detect_sem;
     248             : };
     249             : 
     250             : namespace AP {
     251             :     AP_Proximity *proximity();
     252             : };
     253             : 
     254             : #endif // HAL_PROXIMITY_ENABLED