Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/ArduCopter/afs_copter.cpp
Views: 1798
/*1copter specific AP_AdvancedFailsafe class2*/34#include "Copter.h"56#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED78/*9setup radio_out values for all channels to termination values10*/11void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)12{13if (_terminate_action == TERMINATE_ACTION_LAND) {14copter.set_mode(Mode::Number::LAND, ModeReason::TERMINATE);15} else {16// stop motors17copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);18copter.motors->output();1920// disarm as well21copter.arming.disarm(AP_Arming::Method::AFS);2223// and set all aux channels24SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM);25SRV_Channels::set_output_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::Limit::TRIM);26SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::TRIM);27SRV_Channels::set_output_limit(SRV_Channel::k_ignition, SRV_Channel::Limit::TRIM);28SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);29SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);30}3132SRV_Channels::output_ch_all();33}3435void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)36{37// setup failsafe for all aux channels38SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM);39SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::Limit::TRIM);40SRV_Channels::set_failsafe_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::TRIM);41SRV_Channels::set_failsafe_limit(SRV_Channel::k_ignition, SRV_Channel::Limit::TRIM);42SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);43SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);4445#if FRAME_CONFIG != HELI_FRAME46// setup AP_Motors outputs for failsafe47uint32_t mask = copter.motors->get_motor_mask();48hal.rcout->set_failsafe_pwm(mask, copter.motors->get_pwm_output_min());49#endif50}5152/*53return an AFS_MODE for current control mode54*/55AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)56{57return copter.flightmode->afs_mode();58}5960//to force entering auto mode when datalink loss61void AP_AdvancedFailsafe_Copter::set_mode_auto(void)62{63copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE);64}65#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED666768