Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: tidy setting of sensor status flags

Browse Source
This commit is contained in:
Peter Barker 2021-04-13 13:55:53 +10:00 committed by Andrew Tridgell
parent 187549b431
commit 6727a6588f
1 changed files with 33 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
ArduCopter/GCS_Copter.cpp
View File

@ -42,37 +42,17 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION;
#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
#if PRECISION_LANDING == ENABLED
if (copter.precland.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
const Copter::ap_t &ap = copter.ap;
if (ap.rc_receiver_present) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if HAL_PROXIMITY_ENABLED
if (copter.g2.proximity.sensor_present()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
if (ap.rc_receiver_present && !copter.failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
// update flightmode-specific flags:
control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
@ -106,32 +86,54 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
break;
}
// optional sensors, some of which are essentially always
// available in the firmware:
#if HAL_PROXIMITY_ENABLED
if (copter.g2.proximity.sensor_present()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
if (copter.g2.proximity.sensor_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
if (!copter.g2.proximity.sensor_failed()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
#endif
if (ap.rc_receiver_present && !copter.failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
#if RANGEFINDER_ENABLED == ENABLED
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (copter.rangefinder_state.enabled) {
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 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;
}
if (optflow && optflow->healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if PRECISION_LANDING == ENABLED
if (copter.precland.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
if (copter.precland.enabled() && copter.precland.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
#if HAL_PROXIMITY_ENABLED
if (!copter.g2.proximity.sensor_failed()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
#endif
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
switch (copter.terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
@ -148,13 +150,4 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
break;
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
if (copter.rangefinder_state.enabled) {
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
}