You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
106 lines
3.5 KiB
106 lines
3.5 KiB
#include "GCS_Sub.h" |
|
|
|
#include "Sub.h" |
|
|
|
uint8_t GCS_Sub::sysid_this_mav() const |
|
{ |
|
return sub.g.sysid_this_mav; |
|
} |
|
|
|
void GCS_Sub::update_vehicle_sensor_status_flags() |
|
{ |
|
// mode-specific sensors: |
|
control_sensors_present |= |
|
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | |
|
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | |
|
MAV_SYS_STATUS_SENSOR_YAW_POSITION; |
|
|
|
control_sensors_enabled |= |
|
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | |
|
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | |
|
MAV_SYS_STATUS_SENSOR_YAW_POSITION; |
|
|
|
control_sensors_health |= |
|
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | |
|
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | |
|
MAV_SYS_STATUS_SENSOR_YAW_POSITION; |
|
|
|
control_sensors_present |= |
|
MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | |
|
MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; |
|
|
|
switch (sub.control_mode) { |
|
case ALT_HOLD: |
|
case AUTO: |
|
case GUIDED: |
|
case CIRCLE: |
|
case SURFACE: |
|
case POSHOLD: |
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; |
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; |
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; |
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; |
|
break; |
|
default: |
|
break; |
|
} |
|
|
|
// override the parent class's values for ABSOLUTE_PRESSURE to |
|
// only check internal barometer: |
|
if (sub.ap.depth_sensor_present) { |
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; |
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; |
|
} |
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; // check the internal barometer only |
|
if (sub.sensor_health.depth) { |
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; |
|
} |
|
|
|
#if AP_OPTICALFLOW_ENABLED |
|
const AP_OpticalFlow *optflow = AP::opticalflow(); |
|
if (optflow && optflow->enabled()) { |
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; |
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; |
|
} |
|
if (optflow && optflow->healthy()) { |
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; |
|
} |
|
#endif |
|
|
|
#if AP_TERRAIN_AVAILABLE |
|
switch (sub.terrain.status()) { |
|
case AP_Terrain::TerrainStatusDisabled: |
|
break; |
|
case AP_Terrain::TerrainStatusUnhealthy: |
|
// To-Do: restore unhealthy terrain status reporting once terrain is used in Sub |
|
//control_sensors_present |= MAV_SYS_STATUS_TERRAIN; |
|
//control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; |
|
//break; |
|
case AP_Terrain::TerrainStatusOK: |
|
control_sensors_present |= MAV_SYS_STATUS_TERRAIN; |
|
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; |
|
control_sensors_health |= MAV_SYS_STATUS_TERRAIN; |
|
break; |
|
} |
|
#endif |
|
|
|
#if RANGEFINDER_ENABLED == ENABLED |
|
const RangeFinder *rangefinder = RangeFinder::get_singleton(); |
|
if (sub.rangefinder_state.enabled) { |
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; |
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; |
|
if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) { |
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; |
|
} |
|
} |
|
#endif |
|
} |
|
|
|
#if AP_LTM_TELEM_ENABLED |
|
// avoid building/linking LTM: |
|
void AP_LTM_Telem::init() {}; |
|
#endif |
|
#if AP_DEVO_TELEM_ENABLED |
|
// avoid building/linking Devo: |
|
void AP_DEVO_Telem::init() {}; |
|
#endif
|
|
|