AC_Autorotation: add and use AP_RPM_ENABLED

... and backend-specific equivalents
This commit is contained in:
Peter Barker 2022-07-15 21:53:41 +10:00 committed by Andrew Tridgell
parent ed38aadc72
commit 586f0c45ac
1 changed files with 6 additions and 2 deletions

View File

@ -204,11 +204,12 @@ void AC_Autorotation::set_collective(float collective_filter_cutoff) const
//before using it in the controller
float AC_Autorotation::get_rpm(bool update_counter)
{
float current_rpm = 0.0f;
#if AP_RPM_ENABLED
// Get singleton for RPM library
const AP_RPM *rpm = AP_RPM::get_singleton();
float current_rpm = 0.0f;
//Get current rpm, checking to ensure no nullptr
if (rpm != nullptr) {
//Check requested rpm instance to ensure either 0 or 1. Always defaults to 0.
@ -228,6 +229,9 @@ float AC_Autorotation::get_rpm(bool update_counter)
} else {
_flags.bad_rpm = true;
}
#else
_flags.bad_rpm = true;
#endif
if (_flags.bad_rpm) {
//count unhealthy rpm updates and reset healthy rpm counter