mirror of https://github.com/ArduPilot/ardupilot
113 lines
3.9 KiB
C++
113 lines
3.9 KiB
C++
#include "GCS_Sub.h"
|
|
|
|
#include "Sub.h"
|
|
|
|
void GCS_Sub::update_vehicle_sensor_status_flags()
|
|
{
|
|
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;
|
|
|
|
// first what sensors/controllers we have
|
|
if (sub.g.compass_enabled) {
|
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
|
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
|
|
}
|
|
if (sub.ap.depth_sensor_present) {
|
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
|
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
|
|
}
|
|
const AP_GPS &gps = AP::gps();
|
|
if (gps.status() > AP_GPS::NO_GPS) {
|
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
|
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
|
|
}
|
|
#if OPTFLOW == ENABLED
|
|
const 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;
|
|
}
|
|
#endif
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
const Compass &compass = AP::compass();
|
|
if (sub.g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
|
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
|
|
}
|
|
if (gps.is_healthy()) {
|
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
|
|
}
|
|
#if OPTFLOW == ENABLED
|
|
if (optflow && optflow->healthy()) {
|
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
|
|
}
|
|
#endif
|
|
|
|
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
|
switch (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
|
|
}
|
|
|
|
// avoid building/linking Devo:
|
|
void AP_DEVO_Telem::init() {};
|