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/control_modes.cpp
Views: 1798
1
#include "Plane.h"

2


3
#include "quadplane.h"

4
#include "qautotune.h"

5


6
Mode *Plane::mode_from_mode_num(const enum Mode::Number num)

7
{

8
    Mode *ret = nullptr;

9
    switch (num) {

10
    case Mode::Number::MANUAL:

11
        ret = &mode_manual;

12
        break;

13
    case Mode::Number::CIRCLE:

14
        ret = &mode_circle;

15
        break;

16
    case Mode::Number::STABILIZE:

17
        ret = &mode_stabilize;

18
        break;

19
    case Mode::Number::TRAINING:

20
        ret = &mode_training;

21
        break;

22
    case Mode::Number::ACRO:

23
        ret = &mode_acro;

24
        break;

25
    case Mode::Number::FLY_BY_WIRE_A:

26
        ret = &mode_fbwa;

27
        break;

28
    case Mode::Number::FLY_BY_WIRE_B:

29
        ret = &mode_fbwb;

30
        break;

31
    case Mode::Number::CRUISE:

32
        ret = &mode_cruise;

33
        break;

34
    case Mode::Number::AUTOTUNE:

35
        ret = &mode_autotune;

36
        break;

37
    case Mode::Number::AUTO:

38
        ret = &mode_auto;

39
        break;

40
    case Mode::Number::RTL:

41
        ret = &mode_rtl;

42
        break;

43
    case Mode::Number::LOITER:

44
        ret = &mode_loiter;

45
        break;

46
    case Mode::Number::AVOID_ADSB:

47
#if HAL_ADSB_ENABLED

48
        ret = &mode_avoidADSB;

49
        break;

50
#endif

51
    // if ADSB is not compiled in then fallthrough to guided

52
    case Mode::Number::GUIDED:

53
        ret = &mode_guided;

54
        break;

55
    case Mode::Number::INITIALISING:

56
        ret = &mode_initializing;

57
        break;

58
#if HAL_QUADPLANE_ENABLED

59
    case Mode::Number::QSTABILIZE:

60
        ret = &mode_qstabilize;

61
        break;

62
    case Mode::Number::QHOVER:

63
        ret = &mode_qhover;

64
        break;

65
    case Mode::Number::QLOITER:

66
        ret = &mode_qloiter;

67
        break;

68
    case Mode::Number::QLAND:

69
        ret = &mode_qland;

70
        break;

71
    case Mode::Number::QRTL:

72
        ret = &mode_qrtl;

73
        break;

74
    case Mode::Number::QACRO:

75
        ret = &mode_qacro;

76
        break;

77
#if QAUTOTUNE_ENABLED

78
    case Mode::Number::QAUTOTUNE:

79
        ret = &mode_qautotune;

80
        break;

81
#endif

82
#endif  // HAL_QUADPLANE_ENABLED

83
    case Mode::Number::TAKEOFF:

84
        ret = &mode_takeoff;

85
        break;

86
#if MODE_AUTOLAND_ENABLED

87
    case Mode::Number::AUTOLAND:

88
        ret = &mode_autoland;

89
        break;

90
#endif //MODE_AUTOLAND_ENABLED

91
    case Mode::Number::THERMAL:

92
#if HAL_SOARING_ENABLED

93
        ret = &mode_thermal;

94
#endif

95
        break;

96
#if HAL_QUADPLANE_ENABLED

97
    case Mode::Number::LOITER_ALT_QLAND:

98
        ret = &mode_loiter_qland;

99
        break;

100
#endif  // HAL_QUADPLANE_ENABLED

101


102
    }

103
    return ret;

104
}

105


106
void RC_Channels_Plane::read_mode_switch()

107
{

108
    if (millis() - plane.failsafe.last_valid_rc_ms > 100) {

109
        // only use signals that are less than 0.1s old.

110
        return;

111
    }

112
    RC_Channels::read_mode_switch();

113
}

114


115
void RC_Channel_Plane::mode_switch_changed(modeswitch_pos_t new_pos)

116
{

117
    if (new_pos < 0 || (uint8_t)new_pos > plane.num_flight_modes) {

118
        // should not have been called

119
        return;

120
    }

121


122
    plane.set_mode_by_number((Mode::Number)plane.flight_modes[new_pos].get(), ModeReason::RC_COMMAND);

123
}

124


125
/*

126
  called when entering autotune

127
 */

128
void Plane::autotune_start(void)

129
{

130
    const bool tune_roll = g2.axis_bitmask.get() & int8_t(AutoTuneAxis::ROLL);

131
    const bool tune_pitch = g2.axis_bitmask.get() & int8_t(AutoTuneAxis::PITCH);

132
    const bool tune_yaw = g2.axis_bitmask.get() & int8_t(AutoTuneAxis::YAW);

133
    if (tune_roll || tune_pitch || tune_yaw) {

134
        gcs().send_text(MAV_SEVERITY_INFO, "Started autotune");

135
        if (tune_roll) { 

136
            rollController.autotune_start();

137
        }

138
        if (tune_pitch) { 

139
            pitchController.autotune_start();

140
        }

141
        if (tune_yaw) { 

142
            yawController.autotune_start();

143
        }

144
        autotuning = true;

145
        gcs().send_text(MAV_SEVERITY_INFO, "Autotuning %s%s%s", tune_roll?"roll ":"", tune_pitch?"pitch ":"", tune_yaw?"yaw":"");

146
    } else {

147
        gcs().send_text(MAV_SEVERITY_INFO, "No axis selected for tuning!");

148
    }        

149
}

150


151
/*

152
  called when exiting autotune

153
 */

154
void Plane::autotune_restore(void)

155
{

156
    rollController.autotune_restore();

157
    pitchController.autotune_restore();

158
    yawController.autotune_restore();

159
    if (autotuning) {

160
        autotuning = false;

161
        gcs().send_text(MAV_SEVERITY_INFO, "Stopped autotune");

162
    }

163
}

164


165
/*

166
  enable/disable autotune for AUTO modes

167
 */

168
void Plane::autotune_enable(bool enable)

169
{

170
    if (enable) {

171
        autotune_start();

172
    } else {

173
        autotune_restore();

174
    }

175
}

176


177
/*

178
  are we flying inverted?

179
 */

180
bool Plane::fly_inverted(void)

181
{

182
    if (control_mode == &plane.mode_manual) {

183
        return false;

184
    }

185
    if (inverted_flight) {

186
        // controlled with aux switch

187
        return true;

188
    }

189
    if (control_mode == &mode_auto && auto_state.inverted_flight) {

190
        return true;

191
    }

192
    return false;

193
}

194


195