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_Backend.h"
      17             : 
      18             : #if HAL_PROXIMITY_ENABLED
      19             : #include <AP_Common/Location.h>
      20             : #include <AP_AHRS/AP_AHRS.h>
      21             : #include <AC_Avoidance/AP_OADatabase.h>
      22             : 
      23             : /*
      24             :   base class constructor. 
      25             :   This incorporates initialisation as well.
      26             : */
      27          15 : AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity& _frontend, AP_Proximity::Proximity_State& _state, AP_Proximity_Params& _params) :
      28          15 :         frontend(_frontend),
      29          15 :         state(_state),
      30          15 :         params(_params)
      31             : {
      32          15 :     _backend_type = (AP_Proximity::Type )_params.type.get();
      33          15 : }
      34             : 
      35             : static_assert(PROXIMITY_MAX_DIRECTION <= 8,
      36             :               "get_horizontal_distances assumes 8-bits is enough for validity bitmask");
      37             : 
      38             : // set status and update valid count
      39      138688 : void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
      40             : {
      41      138688 :     state.status = status;
      42      138688 : }
      43             : 
      44             : // correct an angle (in degrees) based on the orientation and yaw correction parameters
      45      256072 : float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const
      46             : {
      47      256072 :     const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;
      48      256072 :     return wrap_360(angle_degrees * angle_sign + params.yaw_correction);
      49             : }
      50             : 
      51             : // 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)
      52             : // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
      53             : // 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)
      54             : // Also checks if obstacle is near land or out of range
      55             : // angles should be in degrees and in the range of 0 to 360, distance should be in meteres
      56      429304 : bool AP_Proximity_Backend::ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area) const
      57             : {
      58             :     // check if distances are supposed to be in a particular range
      59      429304 :     if (!is_zero(params.max_m)) {
      60           0 :         if (distance_m > params.max_m) {
      61             :             // too far away
      62           0 :             return true;
      63             :         }
      64             :     }
      65             : 
      66      429304 :     if (!is_zero(params.min_m)) {
      67           0 :         if (distance_m < params.min_m) {
      68             :             // too close
      69           0 :             return true;
      70             :         }
      71             :     }
      72             : 
      73      429304 :     if (check_for_ign_area) {
      74             :         // check angle vs each ignore area
      75      599320 :         for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
      76      479456 :             if (params.ignore_width_deg[i] != 0) {
      77           0 :                 if (abs(yaw - params.ignore_angle_deg[i]) <= (params.ignore_width_deg[i]/2)) {
      78           0 :                     return true;
      79             :                 }
      80             :             }
      81             :         }
      82             :     }
      83             : 
      84             :    // check if obstacle is near land
      85      429304 :    return frontend.check_obstacle_near_ground(pitch, yaw, distance_m);
      86             : }
      87             : 
      88             : // returns true if database is ready to be pushed to and all cached data is ready
      89      334893 : bool AP_Proximity_Backend::database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned)
      90             : {
      91             : #if AP_OADATABASE_ENABLED
      92      334893 :     AP_OADatabase *oaDb = AP::oadatabase();
      93      334893 :     if (oaDb == nullptr || !oaDb->healthy()) {
      94      315551 :         return false;
      95             :     }
      96             : 
      97       19342 :     if (!AP::ahrs().get_relative_position_NED_origin_float(current_pos)) {
      98       17606 :         return false;
      99             :     }
     100             : 
     101        1736 :     body_to_ned = AP::ahrs().get_rotation_body_to_ned();
     102             : 
     103        1736 :     return true;
     104             : #else
     105             :     return false;
     106             : #endif
     107             : }
     108             : 
     109             : // update Object Avoidance database with Earth-frame point
     110      334514 : void AP_Proximity_Backend::database_push(float angle, float pitch, float distance)
     111             : {
     112      334514 :     Vector3f current_pos;
     113      334514 :     Matrix3f body_to_ned;
     114             : 
     115      334514 :     if (database_prepare_for_push(current_pos, body_to_ned)) {
     116        1736 :         database_push(angle, pitch, distance, AP_HAL::millis(), current_pos, body_to_ned);
     117             :     }
     118      334514 : }
     119             : 
     120             : // update Object Avoidance database with Earth-frame point
     121             : // pitch can be optionally provided if needed
     122        1736 : void AP_Proximity_Backend::database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned)
     123             : {
     124             : 
     125             : #if AP_OADATABASE_ENABLED
     126        1736 :     AP_OADatabase *oaDb = AP::oadatabase();
     127        1736 :     if (oaDb == nullptr || !oaDb->healthy()) {
     128           0 :         return;
     129             :     }
     130        1736 :     if ((pitch > 90.0f) || (pitch < -90.0f)) {
     131             :         // sanity check on pitch
     132           0 :         return;
     133             :     }
     134             :     //Assume object is angle and pitch bearing and distance meters away from the vehicle 
     135        1736 :     Vector3f object_3D;
     136        1736 :     object_3D.offset_bearing(wrap_180(angle), (pitch * -1.0f), distance);
     137        1736 :     const Vector3f rotated_object_3D = body_to_ned * object_3D;
     138             : 
     139             :     //Calculate the position vector from origin
     140        1736 :     Vector3f temp_pos = current_pos + rotated_object_3D;
     141             :     //Convert the vector to a NEU frame from NED
     142        1736 :     temp_pos.z = temp_pos.z * -1.0f;
     143             : 
     144        1736 :     oaDb->queue_push(temp_pos, timestamp_ms, distance, AP_OADatabase::OA_DbItem::Source::proximity);
     145             : #endif  // AP_OADATABASE_ENABLED
     146             : }
     147             : 
     148             : #endif // HAL_PROXIMITY_ENABLED