Line data    Source code

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