Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Ardupilot
GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/ArduPlane/GCS_Plane.cpp
9317 views
1
#include "GCS_Plane.h"

2
#include "Plane.h"

3


4
void GCS_Plane::update_vehicle_sensor_status_flags(void)

5
{

6
    // reverse thrust

7
    if (plane.have_reverse_thrust()) {

8
        control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;

9
    }

10
    if (plane.have_reverse_thrust() && is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))) {

11
        control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;

12
        control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;

13
    }

14


15
    // flightmode-specific

16
    control_sensors_present |=

17
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

18
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

19
        MAV_SYS_STATUS_SENSOR_YAW_POSITION |

20
        MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |

21
        MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

22


23
    bool rate_controlled = false;

24
    bool attitude_stabilized = false;

25
    switch (plane.control_mode->mode_number()) {

26
    case Mode::Number::MANUAL:

27
        break;

28


29
    case Mode::Number::ACRO:

30
#if HAL_QUADPLANE_ENABLED

31
    case Mode::Number::QACRO:

32
#endif

33
        rate_controlled = true;

34
        break;

35


36
    case Mode::Number::STABILIZE:

37
    case Mode::Number::FLY_BY_WIRE_A:

38
    case Mode::Number::AUTOTUNE:

39
#if HAL_QUADPLANE_ENABLED

40
    case Mode::Number::QSTABILIZE:

41
    case Mode::Number::QHOVER:

42
    case Mode::Number::QLAND:

43
    case Mode::Number::QLOITER:

44
#if QAUTOTUNE_ENABLED

45
    case Mode::Number::QAUTOTUNE:

46
#endif

47
#endif  // HAL_QUADPLANE_ENABLED

48
    case Mode::Number::FLY_BY_WIRE_B:

49
    case Mode::Number::CRUISE:

50
        rate_controlled = true;

51
        attitude_stabilized = true;

52
        break;

53


54
    case Mode::Number::TRAINING:

55
        if (!plane.training_manual_roll || !plane.training_manual_pitch) {

56
            rate_controlled = true;

57
            attitude_stabilized = true;

58
        }

59
        break;

60


61
    case Mode::Number::AUTO:

62
    case Mode::Number::RTL:

63
    case Mode::Number::LOITER:

64
    case Mode::Number::AVOID_ADSB:

65
    case Mode::Number::GUIDED:

66
    case Mode::Number::CIRCLE:

67
    case Mode::Number::TAKEOFF:

68
#if MODE_AUTOLAND_ENABLED

69
    case Mode::Number::AUTOLAND:

70
#endif

71
#if HAL_QUADPLANE_ENABLED

72
    case Mode::Number::QRTL:

73
    case Mode::Number::LOITER_ALT_QLAND:

74
#endif

75
    case Mode::Number::THERMAL:

76
        rate_controlled = true;

77
        attitude_stabilized = true;

78
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;

79
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;

80
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

81
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

82
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

83
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

84
        break;

85


86
    case Mode::Number::INITIALISING:

87
        break;

88
    }

89


90
    if (rate_controlled) {

91
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control

92
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control

93
    }

94
    if (attitude_stabilized) {

95
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;

96
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;

97
    }

98


99
#if AP_TERRAIN_AVAILABLE

100
    switch (plane.terrain.status()) {

101
    case AP_Terrain::TerrainStatusDisabled:

102
        break;

103
    case AP_Terrain::TerrainStatusUnhealthy:

104
        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

105
        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

106
        break;

107
    case AP_Terrain::TerrainStatusOK:

108
        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

109
        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

110
        control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;

111
        break;

112
    }

113
#endif

114


115
#if AP_RANGEFINDER_ENABLED

116
    const RangeFinder *rangefinder = RangeFinder::get_singleton();

117
    if (rangefinder && rangefinder->has_orientation(plane.rangefinder_orientation())) {

118
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

119
        if (uint16_t(plane.g.rangefinder_landing.get()) != 0) {

120
            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

121
        }

122
        if (rangefinder->has_data_orient(plane.rangefinder_orientation())) {

123
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;            

124
        }

125
    }

126
#endif

127
}

128


129