Line data    Source code

       1             : #include "GCS_Sub.h"
       2             : 
       3             : #include "Sub.h"
       4             : 
       5       52016 : void GCS_Sub::update_vehicle_sensor_status_flags()
       6             : {
       7             :     // mode-specific sensors:
       8       52016 :     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       52016 :     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       52016 :     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       52016 :     control_sensors_present |=
      24             :         MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |
      25             :         MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      26             : 
      27       52016 :     switch (sub.control_mode) {
      28       25311 :     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       25311 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      35       25311 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      36       25311 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      37       25311 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      38       25311 :         break;
      39       26705 :     default:
      40       26705 :         break;
      41             :     }
      42             : 
      43             :     // override the parent class's values for ABSOLUTE_PRESSURE to
      44             :     // only honour water-pressure sensors
      45       52016 :     control_sensors_present &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
      46       52016 :     control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
      47       52016 :     control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
      48       52016 :     if (sub.ap.depth_sensor_present) {
      49       52016 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
      50       52016 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
      51       52016 :         if (sub.sensor_health.depth) {
      52       51455 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
      53             :         }
      54             :     }
      55             : 
      56             : #if AP_TERRAIN_AVAILABLE
      57       52016 :     switch (sub.terrain.status()) {
      58       52016 :     case AP_Terrain::TerrainStatusDisabled:
      59       52016 :         break;
      60           0 :     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           0 :         control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
      67           0 :         control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
      68           0 :         control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;
      69           0 :         break;
      70             :     }
      71             : #endif
      72             : 
      73             : #if AP_RANGEFINDER_ENABLED
      74       52016 :     const RangeFinder *rangefinder = RangeFinder::get_singleton();
      75       52016 :     if (sub.rangefinder_state.enabled) {
      76       51292 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      77       51292 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      78       51292 :         if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) {
      79       51096 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      80             :         }
      81             :     }
      82             : #endif
      83       52016 : }
      84             : 
      85             : #if AP_LTM_TELEM_ENABLED
      86             : // avoid building/linking LTM:
      87          83 : 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