Line data    Source code

       1             : 
       2             : #include <stdio.h>
       3             : #include "Plane.h"
       4             : 
       5             : #if HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
       6      547089 : void Plane::avoidance_adsb_update(void)
       7             : {
       8             : #if HAL_ADSB_ENABLED
       9      547089 :     adsb.update();
      10             : #endif  // HAL_ADSB_ENABLED
      11             : #if AP_ADSB_AVOIDANCE_ENABLED
      12      547089 :     avoidance_adsb.update();
      13             : #endif  // AP_ADSB_AVOIDANCE_ENABLED
      14      547089 : }
      15             : #endif  // HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
      16             : 
      17             : #if AP_ADSB_AVOIDANCE_ENABLED
      18         117 : MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action)
      19             : {
      20         117 :     MAV_COLLISION_ACTION actual_action = requested_action;
      21         117 :     bool failsafe_state_change = false;
      22             : 
      23             :     // check for changes in failsafe
      24         117 :     if (!plane.failsafe.adsb) {
      25           4 :         plane.failsafe.adsb = true;
      26           4 :         failsafe_state_change = true;
      27             :         // record flight mode in case it's required for the recovery
      28           4 :         prev_control_mode_number = plane.control_mode->mode_number();
      29             :     }
      30             : 
      31             :     // take no action in some flight modes
      32         117 :     bool flightmode_prohibits_action = false;
      33         117 :     if (plane.control_mode == &plane.mode_manual ||
      34         117 :         (plane.control_mode == &plane.mode_auto && !plane.auto_state.takeoff_complete) ||
      35         117 :         (plane.flight_stage == AP_FixedWing::FlightStage::LAND) || // TODO: consider allowing action during approach
      36         117 :         plane.control_mode == &plane.mode_autotune) {
      37           0 :         flightmode_prohibits_action = true;
      38             :     }
      39             : #if HAL_QUADPLANE_ENABLED
      40         117 :     if (plane.control_mode == &plane.mode_qland) {
      41           0 :         flightmode_prohibits_action = true;
      42             :     }
      43             : #endif
      44         117 :     if (flightmode_prohibits_action) {
      45           0 :         actual_action = MAV_COLLISION_ACTION_NONE;
      46             :     }
      47             : 
      48             :     // take action based on requested action
      49         117 :     switch (actual_action) {
      50             : 
      51          40 :         case MAV_COLLISION_ACTION_RTL:
      52          40 :             if (failsafe_state_change) {
      53           1 :                 plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE);
      54             :             }
      55          40 :             break;
      56             : 
      57           0 :         case MAV_COLLISION_ACTION_HOVER:
      58           0 :             if (failsafe_state_change) {
      59             : #if HAL_QUADPLANE_ENABLED
      60           0 :                 if (plane.quadplane.is_flying()) {
      61           0 :                     plane.set_mode(plane.mode_qloiter, ModeReason::AVOIDANCE);
      62           0 :                     break;
      63             :                 }
      64             : #endif
      65           0 :                 plane.set_mode(plane.mode_loiter, ModeReason::AVOIDANCE);
      66             :             }
      67           0 :             break;
      68             : 
      69           0 :         case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: {
      70             :             // climb or descend to avoid obstacle
      71           0 :             Location loc = plane.next_WP_loc;
      72           0 :             if (handle_avoidance_vertical(obstacle, failsafe_state_change, loc)) {
      73           0 :                 plane.set_guided_WP(loc);
      74             :             } else {
      75           0 :                 actual_action = MAV_COLLISION_ACTION_NONE;
      76             :             }
      77           0 :             break;
      78             :         }
      79          77 :         case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: {
      80             :             // move horizontally to avoid obstacle
      81          77 :             Location loc = plane.next_WP_loc;
      82          77 :             if (handle_avoidance_horizontal(obstacle, failsafe_state_change, loc)) {
      83          77 :                 plane.set_guided_WP(loc);
      84             :             } else {
      85           0 :                 actual_action = MAV_COLLISION_ACTION_NONE;
      86             :             }
      87          77 :             break;
      88             :         }
      89           0 :         case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR:
      90             :         {
      91             :             // move horizontally and vertically to avoid obstacle
      92           0 :             Location loc = plane.next_WP_loc;
      93           0 :             const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change, loc);
      94           0 :             const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change, loc);
      95           0 :             if (success_vert || success_hor) {
      96           0 :                 plane.set_guided_WP(loc);
      97             :             } else {
      98           0 :                 actual_action = MAV_COLLISION_ACTION_NONE;
      99             :             }
     100             :         }
     101           0 :             break;
     102             : 
     103             :         // unsupported actions and those that require no response
     104           0 :         case MAV_COLLISION_ACTION_NONE:
     105           0 :             return actual_action;
     106           0 :         case MAV_COLLISION_ACTION_REPORT:
     107             :         default:
     108           0 :             break;
     109             :     }
     110             : 
     111         117 :     if (failsafe_state_change) {
     112           4 :         gcs().send_text(MAV_SEVERITY_ALERT, "Avoid: Performing action: %d", actual_action);
     113             :     }
     114             : 
     115             :     // return with action taken
     116         117 :     return actual_action;
     117             : }
     118             : 
     119           7 : void AP_Avoidance_Plane::handle_recovery(RecoveryAction recovery_action)
     120             : {
     121             :     // check we are coming out of failsafe
     122           7 :     if (plane.failsafe.adsb) {
     123           4 :         plane.failsafe.adsb = false;
     124           4 :         gcs().send_text(MAV_SEVERITY_INFO, "Avoid: Resuming with action: %u", (unsigned)recovery_action);
     125             : 
     126             :         // restore flight mode if requested and user has not changed mode since
     127           4 :         if (plane.control_mode_reason == ModeReason::AVOIDANCE) {
     128           4 :             switch (recovery_action) {
     129             : 
     130           0 :             case RecoveryAction::REMAIN_IN_AVOID_ADSB:
     131             :                 // do nothing, we'll stay in the AVOID_ADSB mode which is guided which will loiter
     132           0 :                 break;
     133             : 
     134           0 :             case RecoveryAction::RESUME_PREVIOUS_FLIGHTMODE:
     135           0 :                 plane.set_mode_by_number(prev_control_mode_number, ModeReason::AVOIDANCE_RECOVERY);
     136           0 :                 break;
     137             : 
     138           0 :             case RecoveryAction::RTL:
     139           0 :                 plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE_RECOVERY);
     140           0 :                 break;
     141             : 
     142           4 :             case RecoveryAction::RESUME_IF_AUTO_ELSE_LOITER:
     143           4 :                 if (prev_control_mode_number == Mode::Number::AUTO) {
     144           1 :                     plane.set_mode(plane.mode_auto, ModeReason::AVOIDANCE_RECOVERY);
     145             :                 } else {
     146             :                     // let ModeAvoidADSB continue in its guided
     147             :                     // behaviour, but reset the loiter location,
     148             :                     // rather than where the avoidance location was
     149           3 :                     plane.set_guided_WP(plane.current_loc);
     150             :                 }
     151           4 :                 break;
     152             : 
     153           0 :             default:
     154             :                 // user has specified an invalid recovery action;
     155             :                 // loiter where we are
     156           0 :                 plane.set_guided_WP(plane.current_loc);
     157           0 :                 break;
     158             :             } // switch
     159             :         }
     160             :     }
     161           7 : }
     162             : 
     163             : // check flight mode is avoid_adsb
     164          77 : bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change)
     165             : {
     166             :     // ensure plane is in avoid_adsb mode
     167          77 :     if (allow_mode_change && plane.control_mode != &plane.mode_avoidADSB) {
     168           2 :         plane.set_mode(plane.mode_avoidADSB, ModeReason::AVOIDANCE);
     169             :     }
     170             : 
     171             :     // check flight mode
     172          77 :     return (plane.control_mode == &plane.mode_avoidADSB);
     173             : }
     174             : 
     175           0 : bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc)
     176             : {
     177             :     // ensure copter is in avoid_adsb mode
     178           0 :      if (!check_flightmode(allow_mode_change)) {
     179           0 :          return false;
     180             :      }
     181             : 
     182             :      // get best vector away from obstacle
     183           0 :      if (plane.current_loc.alt > obstacle->_location.alt) {
     184             :          // should climb, set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX
     185           0 :          new_loc.set_alt_cm(plane.current_loc.alt + 1000, Location::AltFrame::ABSOLUTE);
     186           0 :          return true;
     187             : 
     188           0 :      } else if (plane.current_loc.alt > plane.g.RTL_altitude*100) {
     189             :          // should descend while above RTL alt
     190             :          // TODO: consider using a lower altitude than RTL_altitude since it's default (100m) is quite high
     191           0 :          new_loc.set_alt_cm(plane.current_loc.alt - 1000, Location::AltFrame::ABSOLUTE);
     192           0 :          return true;
     193             :      }
     194             : 
     195           0 :      return false;
     196             : }
     197             : 
     198          77 : bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc)
     199             : {
     200             :     // ensure plane is in avoid_adsb mode
     201          77 :     if (!check_flightmode(allow_mode_change)) {
     202           0 :         return false;
     203             :     }
     204             : 
     205             :     // get best vector away from obstacle
     206          77 :     Vector3f velocity_neu;
     207          77 :     if (get_vector_perpendicular(obstacle, velocity_neu)) {
     208             :         // remove vertical component
     209          77 :         velocity_neu.z = 0.0f;
     210             : 
     211             :         // check for divide by zero
     212          77 :         if (is_zero(velocity_neu.x) && is_zero(velocity_neu.y)) {
     213           0 :             return false;
     214             :         }
     215             : 
     216             :         // re-normalize
     217          77 :         velocity_neu.normalize();
     218             : 
     219             :         // push vector further away.
     220          77 :         velocity_neu *= 10000;
     221             : 
     222             :         // set target
     223          77 :         new_loc.offset(velocity_neu.x, velocity_neu.y);
     224          77 :         return true;
     225             :     }
     226             : 
     227             :     // if we got this far we failed to set the new target
     228           0 :     return false;
     229             : }
     230             : 
     231             : #endif // AP_ADSB_AVOIDANCE_ENABLED