mirror of https://github.com/ArduPilot/ardupilot
96 lines
3.2 KiB
C++
96 lines
3.2 KiB
C++
#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 Mode::Number::ALT_HOLD:
|
|
case Mode::Number::AUTO:
|
|
case Mode::Number::GUIDED:
|
|
case Mode::Number::CIRCLE:
|
|
case Mode::Number::SURFACE:
|
|
case Mode::Number::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_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 AP_RANGEFINDER_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
|