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/ArduPlane/afs_plane.cpp
Views: 1798
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

114


115