Line data    Source code

       1             : #include "Copter.h"
       2             : #include <AP_Notify/AP_Notify.h>
       3             : 
       4             : #if HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
       5      759507 : void Copter::avoidance_adsb_update(void)
       6             : {
       7             : #if HAL_ADSB_ENABLED
       8      759507 :     adsb.update();
       9             : #endif  // HAL_ADSB_ENABLED
      10             : #if AP_ADSB_AVOIDANCE_ENABLED
      11      759507 :     avoidance_adsb.update();
      12             : #endif  // AP_ADSB_AVOIDANCE_ENABLED
      13      759507 : }
      14             : #endif  // HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
      15             : 
      16             : #if AP_ADSB_AVOIDANCE_ENABLED
      17             : 
      18             : #include <stdio.h>
      19             : 
      20          50 : MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action)
      21             : {
      22          50 :     MAV_COLLISION_ACTION actual_action = requested_action;
      23          50 :     bool failsafe_state_change = false;
      24             : 
      25             :     // check for changes in failsafe
      26          50 :     if (!copter.failsafe.adsb) {
      27           2 :         copter.failsafe.adsb = true;
      28           2 :         failsafe_state_change = true;
      29             :         // record flight mode in case it's required for the recovery
      30           2 :         prev_control_mode = copter.flightmode->mode_number();
      31             :     }
      32             : 
      33             :     // take no action in some flight modes
      34         100 :     if (copter.flightmode->mode_number() == Mode::Number::LAND ||
      35             : #if MODE_THROW_ENABLED
      36         100 :         copter.flightmode->mode_number() == Mode::Number::THROW ||
      37             : #endif
      38          50 :         copter.flightmode->mode_number() == Mode::Number::FLIP) {
      39           0 :         actual_action = MAV_COLLISION_ACTION_NONE;
      40             :     }
      41             : 
      42             :     // if landed and we will take some kind of action, just disarm
      43          50 :     if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) {
      44          50 :         copter.arming.disarm(AP_Arming::Method::ADSBCOLLISIONACTION);
      45          50 :         actual_action = MAV_COLLISION_ACTION_NONE;
      46             :     } else {
      47             : 
      48             :         // take action based on requested action
      49           0 :         switch (actual_action) {
      50             : 
      51           0 :             case MAV_COLLISION_ACTION_RTL:
      52             :                 // attempt to switch to RTL, if this fails (i.e. flying in manual mode with bad position) do nothing
      53           0 :                 if (failsafe_state_change) {
      54           0 :                     if (!copter.set_mode(Mode::Number::RTL, ModeReason::AVOIDANCE)) {
      55           0 :                         actual_action = MAV_COLLISION_ACTION_NONE;
      56             :                     }
      57             :                 }
      58           0 :                 break;
      59             : 
      60           0 :             case MAV_COLLISION_ACTION_HOVER:
      61             :                 // attempt to switch to Loiter, if this fails (i.e. flying in manual mode with bad position) do nothing
      62           0 :                 if (failsafe_state_change) {
      63           0 :                     if (!copter.set_mode(Mode::Number::LOITER, ModeReason::AVOIDANCE)) {
      64           0 :                         actual_action = MAV_COLLISION_ACTION_NONE;
      65             :                     }
      66             :                 }
      67           0 :                 break;
      68             : 
      69           0 :             case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND:
      70             :                 // climb or descend to avoid obstacle
      71           0 :                 if (!handle_avoidance_vertical(obstacle, failsafe_state_change)) {
      72           0 :                     actual_action = MAV_COLLISION_ACTION_NONE;
      73             :                 }
      74           0 :                 break;
      75             : 
      76           0 :             case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY:
      77             :                 // move horizontally to avoid obstacle
      78           0 :                 if (!handle_avoidance_horizontal(obstacle, failsafe_state_change)) {
      79           0 :                     actual_action = MAV_COLLISION_ACTION_NONE;
      80             :                 }
      81           0 :                 break;
      82             : 
      83           0 :             case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR:
      84           0 :                 if (!handle_avoidance_perpendicular(obstacle, failsafe_state_change)) {
      85           0 :                     actual_action = MAV_COLLISION_ACTION_NONE;
      86             :                 }
      87           0 :                 break;
      88             : 
      89             :             // unsupported actions and those that require no response
      90           0 :             case MAV_COLLISION_ACTION_NONE:
      91           0 :                 return actual_action;
      92           0 :             case MAV_COLLISION_ACTION_REPORT:
      93             :             default:
      94           0 :                 break;
      95             :         }
      96             :     }
      97             : 
      98             : #if HAL_LOGGING_ENABLED
      99          50 :     if (failsafe_state_change) {
     100           2 :         AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_ADSB,
     101             :                                  LogErrorCode(actual_action));
     102             :     }
     103             : #endif
     104             : 
     105             :     // return with action taken
     106          50 :     return actual_action;
     107             : }
     108             : 
     109           2 : void AP_Avoidance_Copter::handle_recovery(RecoveryAction recovery_action)
     110             : {
     111             :     // check we are coming out of failsafe
     112           2 :     if (copter.failsafe.adsb) {
     113           2 :         copter.failsafe.adsb = false;
     114           2 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_ADSB,
     115             :                            LogErrorCode::ERROR_RESOLVED);
     116             : 
     117             :         // restore flight mode if requested and user has not changed mode since
     118           2 :         if (copter.control_mode_reason == ModeReason::AVOIDANCE) {
     119           0 :             switch (recovery_action) {
     120             : 
     121           0 :             case RecoveryAction::REMAIN_IN_AVOID_ADSB:
     122             :                 // do nothing, we'll stay in the AVOID_ADSB mode which is guided which will loiter forever
     123           0 :                 break;
     124             : 
     125           0 :             case RecoveryAction::RESUME_PREVIOUS_FLIGHTMODE:
     126           0 :                 set_mode_else_try_RTL_else_LAND(prev_control_mode);
     127           0 :                 break;
     128             : 
     129           0 :             case RecoveryAction::RTL:
     130           0 :                 set_mode_else_try_RTL_else_LAND(Mode::Number::RTL);
     131           0 :                 break;
     132             : 
     133           0 :             case RecoveryAction::RESUME_IF_AUTO_ELSE_LOITER:
     134           0 :                 if (prev_control_mode == Mode::Number::AUTO) {
     135           0 :                     set_mode_else_try_RTL_else_LAND(Mode::Number::AUTO);
     136             :                 }
     137           0 :                 break;
     138             : 
     139           0 :             default:
     140           0 :                 break;
     141             :             } // switch
     142             :         }
     143             :     }
     144           2 : }
     145             : 
     146           0 : void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode)
     147             : {
     148           0 :     if (!copter.set_mode(mode, ModeReason::AVOIDANCE_RECOVERY)) {
     149             :         // on failure RTL or LAND
     150           0 :         if (!copter.set_mode(Mode::Number::RTL, ModeReason::AVOIDANCE_RECOVERY)) {
     151           0 :             copter.set_mode(Mode::Number::LAND, ModeReason::AVOIDANCE_RECOVERY);
     152             :         }
     153             :     }
     154           0 : }
     155             : 
     156           0 : float AP_Avoidance_Copter::get_altitude_minimum_m() const
     157             : {
     158             : #if MODE_RTL_ENABLED
     159             :     // do not descend if below RTL_ALT_M
     160           0 :     return copter.mode_rtl.get_altitude_m();
     161             : #else
     162             :     return 0;
     163             : #endif
     164             : }
     165             : 
     166             : // check flight mode is avoid_adsb
     167           0 : bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change)
     168             : {
     169             :     // ensure copter is in avoid_adsb mode
     170           0 :     if (allow_mode_change && copter.flightmode->mode_number() != Mode::Number::AVOID_ADSB) {
     171           0 :         if (!copter.set_mode(Mode::Number::AVOID_ADSB, ModeReason::AVOIDANCE)) {
     172             :             // failed to set mode so exit immediately
     173           0 :             return false;
     174             :         }
     175             :     }
     176             : 
     177             :     // check flight mode
     178           0 :     return (copter.flightmode->mode_number() == Mode::Number::AVOID_ADSB);
     179             : }
     180             : 
     181           0 : bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
     182             : {
     183             :     // ensure copter is in avoid_adsb mode
     184           0 :     if (!check_flightmode(allow_mode_change)) {
     185           0 :         return false;
     186             :     }
     187             : 
     188             :     // decide on whether we should climb or descend
     189           0 :     bool should_climb = false;
     190           0 :     Location my_loc;
     191           0 :     if (AP::ahrs().get_location(my_loc)) {
     192           0 :         should_climb = my_loc.alt > obstacle->_location.alt;
     193             :     }
     194             : 
     195             :     // get best vector away from obstacle
     196           0 :     Vector3f velocity_neu_ms;
     197           0 :     if (should_climb) {
     198           0 :         velocity_neu_ms.z = copter.wp_nav->get_default_speed_up_ms();
     199             :     } else {
     200           0 :         velocity_neu_ms.z = -copter.wp_nav->get_default_speed_down_ms();
     201             :         // do not descend if below minimum altitude
     202           0 :         if (copter.current_loc.alt * 0.01 < get_altitude_minimum_m()) {
     203           0 :             velocity_neu_ms.z = 0.0f;
     204             :         }
     205             :     }
     206             : 
     207             :     // send target velocity
     208           0 :     copter.mode_avoid_adsb.set_velocity_NEU_ms(velocity_neu_ms);
     209           0 :     return true;
     210             : }
     211             : 
     212           0 : bool AP_Avoidance_Copter::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
     213             : {
     214             :     // ensure copter is in avoid_adsb mode
     215           0 :     if (!check_flightmode(allow_mode_change)) {
     216           0 :         return false;
     217             :     }
     218             : 
     219             :     // get best vector away from obstacle
     220           0 :     Vector3f velocity_neu_ms;
     221           0 :     if (get_vector_perpendicular(obstacle, velocity_neu_ms)) {
     222             :         // remove vertical component
     223           0 :         velocity_neu_ms.z = 0.0f;
     224             :         // check for divide by zero
     225           0 :         if (is_zero(velocity_neu_ms.x) && is_zero(velocity_neu_ms.y)) {
     226           0 :             return false;
     227             :         }
     228             :         // re-normalise
     229           0 :         velocity_neu_ms.normalize();
     230             :         // convert horizontal components to velocities
     231           0 :         velocity_neu_ms.x *= copter.wp_nav->get_default_speed_NE_ms();
     232           0 :         velocity_neu_ms.y *= copter.wp_nav->get_default_speed_NE_ms();
     233             :         // send target velocity
     234           0 :         copter.mode_avoid_adsb.set_velocity_NEU_ms(velocity_neu_ms);
     235           0 :         return true;
     236             :     }
     237             : 
     238             :     // if we got this far we failed to set the new target
     239           0 :     return false;
     240             : }
     241             : 
     242           0 : bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
     243             : {
     244             :     // ensure copter is in avoid_adsb mode
     245           0 :     if (!check_flightmode(allow_mode_change)) {
     246           0 :         return false;
     247             :     }
     248             : 
     249             :     // get best vector away from obstacle
     250           0 :     Vector3f velocity_neu_ms;
     251           0 :     if (get_vector_perpendicular(obstacle, velocity_neu_ms)) {
     252             :         // convert horizontal components to velocities
     253           0 :         velocity_neu_ms.x *= copter.wp_nav->get_default_speed_NE_ms();
     254           0 :         velocity_neu_ms.y *= copter.wp_nav->get_default_speed_NE_ms();
     255             :         // use up and down waypoint speeds
     256           0 :         if (velocity_neu_ms.z > 0.0f) {
     257           0 :             velocity_neu_ms.z *= copter.wp_nav->get_default_speed_up_ms();
     258             :         } else {
     259           0 :             velocity_neu_ms.z *= copter.wp_nav->get_default_speed_down_ms();
     260             :             // do not descend if below minimum altitude
     261           0 :             if (copter.current_loc.alt * 0.01 < get_altitude_minimum_m()) {
     262           0 :                 velocity_neu_ms.z = 0.0f;
     263             :             }
     264             :         }
     265             :         // send target velocity
     266           0 :         copter.mode_avoid_adsb.set_velocity_NEU_ms(velocity_neu_ms);
     267           0 :         return true;
     268             :     }
     269             : 
     270             :     // if we got this far we failed to set the new target
     271           0 :     return false;
     272             : }
     273             : 
     274             : #endif  // AP_ADSB_AVOIDANCE_ENABLED