ArduPlane/afs_plane.cpp

   1 /*
   2   plane specific AP_AdvancedFailsafe class
   3  */
   4
   5 #include "Plane.h"
   6
   7 #if AP_ADVANCEDFAILSAFE_ENABLED
   8
   9 /*
  10   setup radio_out values for all channels to termination values
  11  */
  12 void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
  13 {
  14 #if HAL_QUADPLANE_ENABLED
  15     if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) {
  16         // perform a VTOL landing
  17         plane.set_mode(plane.mode_qland, ModeReason::FENCE_BREACHED);
  18         return;
  19     }
  20 #endif
  21
  22     plane.g2.servo_channels.disable_passthrough(true);
  23
  24     if (_terminate_action == TERMINATE_ACTION_LAND) {
  25         plane.landing.terminate();
  26     } else {
  27         // remove flap slew limiting
  28         SRV_Channels::set_slew_rate(SRV_Channel::k_flap_auto, 0.0, 100, plane.G_Dt);
  29         SRV_Channels::set_slew_rate(SRV_Channel::k_flap, 0.0, 100, plane.G_Dt);
  30
  31         // aerodynamic termination is the default approach to termination
  32         SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100.0);
  33         SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100.0);
  34         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX);
  35         SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX);
  36         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX);
  37         if (plane.have_reverse_thrust()) {
  38             // configured for reverse thrust, use TRIM
  39             SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM);
  40             SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::TRIM);
  41             SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::TRIM);
  42         } else {
  43             // use MIN
  44             SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::MIN);
  45             SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::MIN);
  46             SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::MIN);
  47         }
  48         SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);
  49         SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);
  50     }
  51
  52     plane.servos_output();
  53
  54 #if HAL_QUADPLANE_ENABLED
  55     plane.quadplane.afs_terminate();
  56 #endif
  57
  58     // also disarm to ensure that ignition is cut
  59     plane.arming.disarm(AP_Arming::Method::AFS);
  60 }
  61
  62 void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
  63 {
  64     // all aux channels
  65     SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap_auto, SRV_Channel::Limit::MAX);
  66     SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap, SRV_Channel::Limit::MAX);
  67     SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::Limit::MIN);
  68     SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::Limit::MAX);
  69     SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::Limit::MAX);
  70     if (plane.have_reverse_thrust()) {
  71         // configured for reverse thrust, use TRIM
  72         SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM);
  73         SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::TRIM);
  74         SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::TRIM);
  75     } else {
  76         // normal throttle, use MIN
  77         SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::MIN);
  78         SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::MIN);
  79         SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::MIN);
  80     }
  81     SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);
  82     SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);
  83
  84 #if HAL_QUADPLANE_ENABLED
  85     if (plane.quadplane.available()) {
  86         // setup AP_Motors outputs for failsafe
  87         uint32_t mask = plane.quadplane.motors->get_motor_mask();
  88         hal.rcout->set_failsafe_pwm(mask, plane.quadplane.motors->get_pwm_output_min());
  89     }
  90 #endif
  91 }
  92
  93 /*
  94   return an AFS_MODE for current control mode
  95  */
  96 AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void)
  97 {
  98     if (plane.control_mode->does_auto_throttle()) {
  99         return AP_AdvancedFailsafe::AFS_AUTO;
 100     }
 101     if (plane.control_mode == &plane.mode_manual) {
 102         return AP_AdvancedFailsafe::AFS_MANUAL;
 103     }
 104     return AP_AdvancedFailsafe::AFS_STABILIZED;
 105 }
 106
 107 //to force entering auto mode when datalink loss
 108  void AP_AdvancedFailsafe_Plane::set_mode_auto(void)
 109  {
 110     plane.set_mode(plane.mode_auto,ModeReason::GCS_FAILSAFE);
 111  }
 112
 113 #endif // AP_ADVANCEDFAILSAFE_ENABLED