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

2


3
#include "Copter.h"

4


5
uint8_t GCS_Copter::sysid_this_mav() const

6
{

7
    return copter.g.sysid_this_mav;

8
}

9


10
const char* GCS_Copter::frame_string() const

11
{

12
    if (copter.motors == nullptr) {

13
        return "MultiCopter";

14
    }

15
    return copter.motors->get_frame_string();

16
}

17


18
bool GCS_Copter::simple_input_active() const

19
{

20
    return copter.simple_mode == Copter::SimpleMode::SIMPLE;

21
}

22


23
bool GCS_Copter::supersimple_input_active() const

24
{

25
    return copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE;

26
}

27


28
void GCS_Copter::update_vehicle_sensor_status_flags(void)

29
{

30
    control_sensors_present |=

31
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

32
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

33
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

34


35
    control_sensors_enabled |=

36
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

37
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

38
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

39


40
    control_sensors_health |=

41
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

42
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

43
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

44


45
    // Update position controller flags

46
    if (copter.pos_control != nullptr) {

47
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

48
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

49


50
        // XY position controller

51
        if (copter.pos_control->is_active_xy()) {

52
            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

53
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

54
        }

55


56
        // Z altitude controller

57
        if (copter.pos_control->is_active_z()) {

58
            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

59
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

60
        }

61
    }

62


63
    // optional sensors, some of which are essentially always

64
    // available in the firmware:

65
#if HAL_PROXIMITY_ENABLED

66
    if (copter.g2.proximity.sensor_present()) {

67
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROXIMITY;

68
    }

69
    if (copter.g2.proximity.sensor_enabled()) {

70
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROXIMITY;

71
    }

72
    if (!copter.g2.proximity.sensor_failed()) {

73
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY;

74
    }

75
#endif

76


77
#if AP_RANGEFINDER_ENABLED

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

79
    if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {

80
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

81
    }

82
    if (copter.rangefinder_state.enabled) {

83
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

84
        if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) {

85
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

86
        }

87
    }

88
#endif

89


90
#if AC_PRECLAND_ENABLED

91
    if (copter.precland.enabled()) {

92
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;

93
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;

94
    }

95
    if (copter.precland.enabled() && copter.precland.healthy()) {

96
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;

97
    }

98
#endif

99


100
#if AP_TERRAIN_AVAILABLE

101
    switch (copter.terrain.status()) {

102
    case AP_Terrain::TerrainStatusDisabled:

103
        break;

104
    case AP_Terrain::TerrainStatusUnhealthy:

105
        // To-Do: restore unhealthy terrain status reporting once terrain is used in copter

106
        //control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

107
        //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

108
        //break;

109
    case AP_Terrain::TerrainStatusOK:

110
        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

111
        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

112
        control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;

113
        break;

114
    }

115
#endif

116


117
    control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROPULSION;

118
    control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROPULSION;

119
    // only mark propulsion healthy if all of the motors are producing

120
    // nominal thrust

121
    if (!copter.motors->get_thrust_boost()) {

122
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROPULSION;

123
    }

124
}

125


126