Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: use MAV_SYS_STATUS_SENSOR_PROXIMITY for proximity sensor

Browse Source 
Copter: correct laser sensor health bits
This commit is contained in:
Peter Barker 2018-06-25 19:50:14 +10:00 committed by Andrew Tridgell
parent e7e0ee0b50
commit 724f34c7e7
1 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduCopter/sensors.cpp
View File

@ -257,8 +257,8 @@ void Copter::update_sensor_status_flags(void)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (copter.g2.proximity.sensor_present()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
#endif
#if AC_FENCE == ENABLED
@ -266,14 +266,21 @@ void Copter::update_sensor_status_flags(void)
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
if (rangefinder.has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
// all present sensors enabled by default except altitude and position control and motors which we will set individually
// all sensors are present except these, which may be set as enabled below:
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
~MAV_SYS_STATUS_LOGGING &
~MAV_SYS_STATUS_SENSOR_BATTERY &
~MAV_SYS_STATUS_GEOFENCE);
~MAV_SYS_STATUS_GEOFENCE &
~MAV_SYS_STATUS_SENSOR_LASER_POSITION &
~MAV_SYS_STATUS_SENSOR_PROXIMITY);
switch (control_mode) {
case AUTO:
@ -319,6 +326,11 @@ void Copter::update_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.sensor_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
#endif
// default to all healthy
@ -368,8 +380,8 @@ void Copter::update_sensor_status_flags(void)
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.get_status() == AP_Proximity::Proximity_NoData) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (copter.g2.proximity.sensor_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
#endif
@ -392,10 +404,9 @@ void Copter::update_sensor_status_flags(void)
#if RANGEFINDER_ENABLED == ENABLED
if (rangefinder_state.enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (rangefinder.has_data_orient(ROTATION_PITCH_270)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (!rangefinder_state.alt_healthy) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif