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

2


3
#include "Sub.h"

4


5
void GCS_Sub::update_vehicle_sensor_status_flags()

6
{

7
    // mode-specific sensors:

8
    control_sensors_present |=

9
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

10
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

11
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

12


13
    control_sensors_enabled |=

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_health |=

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_present |=

24
        MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |

25
        MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

26


27
    switch (sub.control_mode) {

28
    case Mode::Number::ALT_HOLD:

29
    case Mode::Number::AUTO:

30
    case Mode::Number::GUIDED:

31
    case Mode::Number::CIRCLE:

32
    case Mode::Number::SURFACE:

33
    case Mode::Number::POSHOLD:

34
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

35
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;

36
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

37
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

38
        break;

39
    default:

40
        break;

41
    }

42


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

44
    // only honour water-pressure sensors

45
    control_sensors_present &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

46
    control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

47
    control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

48
    if (sub.ap.depth_sensor_present) {

49
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

50
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

51
        if (sub.sensor_health.depth) {

52
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

53
        }

54
    }

55


56
#if AP_TERRAIN_AVAILABLE

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

58
    case AP_Terrain::TerrainStatusDisabled:

59
        break;

60
    case AP_Terrain::TerrainStatusUnhealthy:

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

62
        //control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

63
        //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

64
        //break;

65
    case AP_Terrain::TerrainStatusOK:

66
        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;

67
        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;

68
        control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;

69
        break;

70
    }

71
#endif

72


73
#if AP_RANGEFINDER_ENABLED

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

75
    if (sub.rangefinder_state.enabled) {

76
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

77
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

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

79
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

80
        }

81
    }

82
#endif

83
}

84


85
#if AP_LTM_TELEM_ENABLED

86
// avoid building/linking LTM:

87
void AP_LTM_Telem::init() {};

88
#endif

89
#if AP_DEVO_TELEM_ENABLED

90
// avoid building/linking Devo:

91
void AP_DEVO_Telem::init() {};

92
#endif

93


94