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

2


3
#include "Sub.h"

4


5
uint8_t GCS_Sub::sysid_this_mav() const

6
{

7
    return sub.g.sysid_this_mav;

8
}

9


10
void GCS_Sub::update_vehicle_sensor_status_flags()

11
{

12
    // mode-specific sensors:

13
    control_sensors_present |=

14
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

15
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

16
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

17


18
    control_sensors_enabled |=

19
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

20
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

21
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

22


23
    control_sensors_health |=

24
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

25
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

26
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

27


28
    control_sensors_present |=

29
        MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |

30
        MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

31


32
    switch (sub.control_mode) {

33
    case Mode::Number::ALT_HOLD:

34
    case Mode::Number::AUTO:

35
    case Mode::Number::GUIDED:

36
    case Mode::Number::CIRCLE:

37
    case Mode::Number::SURFACE:

38
    case Mode::Number::POSHOLD:

39
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

40
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

41
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

42
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

43
        break;

44
    default:

45
        break;

46
    }

47


48
    // override the parent class's values for ABSOLUTE_PRESSURE to

49
    // only check internal barometer:

50
    if (sub.ap.depth_sensor_present) {

51
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

52
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

53
    }

54
    control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; // check the internal barometer only

55
    if (sub.sensor_health.depth) {

56
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

57
    }

58


59
#if AP_TERRAIN_AVAILABLE

60
    switch (sub.terrain.status()) {

61
    case AP_Terrain::TerrainStatusDisabled:

62
        break;

63
    case AP_Terrain::TerrainStatusUnhealthy:

64
        // To-Do: restore unhealthy terrain status reporting once terrain is used in Sub

65
        //control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

66
        //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

67
        //break;

68
    case AP_Terrain::TerrainStatusOK:

69
        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

70
        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

71
        control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;

72
        break;

73
    }

74
#endif

75


76
#if AP_RANGEFINDER_ENABLED

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

78
    if (sub.rangefinder_state.enabled) {

79
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

80
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

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

82
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

83
        }

84
    }

85
#endif

86
}

87


88
#if AP_LTM_TELEM_ENABLED

89
// avoid building/linking LTM:

90
void AP_LTM_Telem::init() {};

91
#endif

92
#if AP_DEVO_TELEM_ENABLED

93
// avoid building/linking Devo:

94
void AP_DEVO_Telem::init() {};

95
#endif

96


97