diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 1dbcccd29423a..9c1be27e07e8a 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -115,6 +115,7 @@ AC_Autorotation::AC_Autorotation() : AP_Param::setup_object_defaults(this, var_info); } + // Initialisation of head speed controller void AC_Autorotation::init_hs_controller() { @@ -374,3 +375,39 @@ float AC_Autorotation::calc_speed_forward(void) return speed_forward; } +// Arming checks for autorotation, mostly checking for miss-configurations +bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const +{ + if (!enabled()) { + // Don't run arming checks if not enabled + return true; + } + + const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + + // Check for correct RPM sensor config +#if AP_RPM_ENABLED + // Get singleton for RPM library + const AP_RPM *rpm = AP_RPM::get_singleton(); + + // Get current rpm, checking to ensure no nullptr + if (rpm == nullptr) { + hal.util->snprintf(buffer, buflen, "Can't access RPM"); + return false; + } + + // Sanity check that the designated rpm sensor instance is there + if ((_param_rpm_instance.get() < 0)) { + hal.util->snprintf(buffer, buflen, "RPM instance <0"); + return false; + } + + if (!rpm->enabled(_param_rpm_instance.get())) { + hal.util->snprintf(buffer, buflen, "RPM%i not enabled", _param_rpm_instance.get()+1); + return false; + } +#endif + + return true; +} + diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 612986662ecf4..ec6ace93e74ce 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -18,6 +18,16 @@ class AC_Autorotation AC_Autorotation(); //--------Functions-------- + bool enabled(void) const { return _param_enable.get() > 0; } + + + + // Arming checks for autorotation, mostly checking for miss-configurations + bool arming_checks(size_t buflen, char *buffer) const; + + + + // not yet checked void init_hs_controller(void); // Initialise head speed controller void init_fwd_spd_controller(void); // Initialise forward speed controller bool update_hs_glide_controller(float dt); // Update head speed controller @@ -29,7 +39,6 @@ class AC_Autorotation float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } float get_last_collective() const { return _collective_out; } - bool is_enable(void) { return _param_enable; } void Log_Write_Autorotation(void) const; void update_forward_speed_controller(void); // Update forward speed controller void set_desired_fwd_speed(void) { _vel_target = _param_target_speed; } // Overloaded: Set desired speed for forward controller to parameter value