CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/ArduCopter/afs_copter.cpp
Views: 1798
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
67
68