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/Rover/failsafe.cpp
Views: 1798
1
/*

2
  failsafe support

3
  Andrew Tridgell, December 2011

4
 */

5


6
#include "Rover.h"

7


8
#include <stdio.h>

9


10
/*

11
  our failsafe strategy is to detect main loop lockup and disarm.

12
 */

13


14
/*

15
  this failsafe_check function is called from the core timer interrupt

16
  at 1kHz.

17
 */

18
void Rover::failsafe_check()

19
{

20
    static uint16_t last_ticks;

21
    static uint32_t last_timestamp;

22
    const uint32_t tnow = AP_HAL::micros();

23


24
    const uint16_t ticks = scheduler.ticks();

25
    if (ticks != last_ticks) {

26
        // the main loop is running, all is OK

27
        last_ticks = ticks;

28
        last_timestamp = tnow;

29
        return;

30
    }

31


32
    if (tnow - last_timestamp > 200000) {

33
        // we have gone at least 0.2 seconds since the main loop

34
        // ran. That means we're in trouble, or perhaps are in

35
        // an initialisation routine or log erase. disarm the motors

36
        // To-Do: log error

37
        if (arming.is_armed()) {

38
            // disarm motors

39
            arming.disarm(AP_Arming::Method::CPUFAILSAFE);

40
        }

41
    }

42
}

43


44
/*

45
  called to set/unset a failsafe event.

46
 */

47
void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool on)

48
{

49
    uint8_t old_bits = failsafe.bits;

50
    if (on) {

51
        failsafe.bits |= failsafe_type;

52
    } else {

53
        failsafe.bits &= ~failsafe_type;

54
    }

55
    if (old_bits == 0 && failsafe.bits != 0) {

56
        // a failsafe event has started

57
        failsafe.start_time = millis();

58
    }

59
    if (failsafe.triggered != 0 && failsafe.bits == 0) {

60
        // a failsafe event has ended

61
        gcs().send_text(MAV_SEVERITY_INFO, "%s Failsafe Cleared", type_str);

62
    }

63


64
    failsafe.triggered &= failsafe.bits;

65


66
    if ((failsafe.triggered == 0) &&

67
        (failsafe.bits != 0) &&

68
        (millis() - failsafe.start_time > g.fs_timeout * 1000) &&

69
        (control_mode != &mode_rtl) &&

70
        ((control_mode != &mode_hold || (g2.fs_options & (uint32_t)Failsafe_Options::Failsafe_Option_Active_In_Hold)))) {

71
        failsafe.triggered = failsafe.bits;

72
        gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe", type_str);

73


74
        // clear rc overrides

75
        RC_Channels::clear_overrides();

76


77
        if ((control_mode == &mode_auto) &&

78
            ((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||

79
             (failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {

80
            // continue with mission in auto mode

81
            gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");

82
        } else {

83
            switch ((FailsafeAction)g.fs_action.get()) {

84
            case FailsafeAction::None:

85
                break;

86
            case FailsafeAction::SmartRTL:

87
                if (set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {

88
                    break;

89
                }

90
                FALLTHROUGH;

91
            case FailsafeAction::RTL:

92
                if (set_mode(mode_rtl, ModeReason::FAILSAFE)) {

93
                    break;

94
                }

95
                FALLTHROUGH;

96
            case FailsafeAction::Hold:

97
                set_mode(mode_hold, ModeReason::FAILSAFE);

98
                break;

99
            case FailsafeAction::SmartRTL_Hold:

100
                if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {

101
                    set_mode(mode_hold, ModeReason::FAILSAFE);

102
                }

103
                break;

104
            case FailsafeAction::Terminate:

105
                arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);

106
                break;

107
            }

108
        }

109
    }

110
}

111


112
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)

113
{

114
        switch ((FailsafeAction)action) {

115
            case FailsafeAction::None:

116
                break;

117
            case FailsafeAction::SmartRTL:

118
                if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {

119
                    break;

120
                }

121
                FALLTHROUGH;

122
            case FailsafeAction::RTL:

123
                if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {

124
                    break;

125
                }

126
                FALLTHROUGH;

127
            case FailsafeAction::Hold:

128
                set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);

129
                break;

130
            case FailsafeAction::SmartRTL_Hold:

131
                if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {

132
                    set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);

133
                }

134
                break;

135
            case FailsafeAction::Terminate:

136
#if AP_ROVER_ADVANCED_FAILSAFE_ENABLED

137
                char battery_type_str[17];

138
                snprintf(battery_type_str, 17, "%s battery", type_str);

139
                g2.afs.gcs_terminate(true, battery_type_str);

140
#else

141
                arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);

142
#endif // AP_ROVER_ADVANCED_FAILSAFE_ENABLED

143
                break;

144
        }

145
}

146


147
#if AP_ROVER_ADVANCED_FAILSAFE_ENABLED

148
/*

149
   check for AFS failsafe check

150
 */

151
void Rover::afs_fs_check(void)

152
{

153
    // perform AFS failsafe checks

154
    g2.afs.check(failsafe.last_valid_rc_ms);

155
}

156
#endif

157


158