ArduCopter/avoidance_adsb.cpp

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