Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: tidy setting of sensor status flags

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

40
ArduPlane/GCS_Plane.cpp
View File

@ -8,27 +8,28 @@ uint8_t GCS_Plane::sysid_this_mav() const
void GCS_Plane::update_vehicle_sensor_status_flags(void)
{
// first what sensors/controllers we have
// airspeed
const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (airspeed && airspeed->enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#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 (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (airspeed && airspeed->enabled() && airspeed->use()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
if (airspeed && airspeed->all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
// reverse thrust
if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (plane.have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
// flightmode-specific
control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
@ -101,13 +102,15 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
}
#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 (airspeed && airspeed->all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
@ -141,9 +144,4 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
if (plane.have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
}