ArduCopter/afs_copter.cpp

   1 /*
   2   copter specific AP_AdvancedFailsafe class
   3  */
   4
   5 #include "Copter.h"
   6
   7 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
   8
   9 /*
  10   setup radio_out values for all channels to termination values
  11  */
  12 void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
  13 {
  14     if (_terminate_action == TERMINATE_ACTION_LAND) {
  15         copter.set_mode(Mode::Number::LAND, ModeReason::TERMINATE);
  16     } else {
  17         // stop motors
  18         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
  19         copter.motors->output();
  20
  21         // disarm as well
  22         copter.arming.disarm(AP_Arming::Method::AFS);
  23
  24         // and set all aux channels
  25         SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM);
  26         SRV_Channels::set_output_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::Limit::TRIM);
  27         SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::TRIM);
  28         SRV_Channels::set_output_limit(SRV_Channel::k_ignition, SRV_Channel::Limit::TRIM);
  29         SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);
  30         SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);
  31     }
  32
  33     SRV_Channels::output_ch_all();
  34 }
  35
  36 void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
  37 {
  38     // setup failsafe for all aux channels
  39     SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM);
  40     SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::Limit::TRIM);
  41     SRV_Channels::set_failsafe_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::TRIM);
  42     SRV_Channels::set_failsafe_limit(SRV_Channel::k_ignition, SRV_Channel::Limit::TRIM);
  43     SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);
  44     SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);
  45
  46 #if FRAME_CONFIG != HELI_FRAME
  47     // setup AP_Motors outputs for failsafe
  48     uint32_t mask = copter.motors->get_motor_mask();
  49     hal.rcout->set_failsafe_pwm(mask, copter.motors->get_pwm_output_min());
  50 #endif
  51 }
  52
  53 /*
  54   return an AFS_MODE for current control mode
  55  */
  56 AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
  57 {
  58     return copter.flightmode->afs_mode();
  59 }
  60
  61 //to force entering auto mode when datalink loss
  62  void AP_AdvancedFailsafe_Copter::set_mode_auto(void)
  63  {
  64     copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE);
  65  }
  66 #endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED