diff --git a/libraries/AC_Autorotation/RSC_Autorotation.cpp b/libraries/AC_Autorotation/RSC_Autorotation.cpp index 84831d48fb7301..19218276cd85d6 100644 --- a/libraries/AC_Autorotation/RSC_Autorotation.cpp +++ b/libraries/AC_Autorotation/RSC_Autorotation.cpp @@ -1,8 +1,11 @@ #include "RSC_Autorotation.h" #include +#include #define RSC_AROT_RAMP_TIME_DEFAULT 2 // time in seconds to ramp motors when bailing out of autorotation +extern const AP_HAL::HAL& hal; + // RSC autorotation state specific parameters const AP_Param::GroupInfo RSC_Autorotation::var_info[] = { @@ -137,3 +140,14 @@ float RSC_Autorotation::get_runup_time(void) const // Never let the runup timer be less than the throttle ramp time return (float) MAX(bailout_throttle_time.get() + 1, bailout_runup_time.get()); } + +// sanity check of parameters, should be called only whilst disarmed +bool RSC_Autorotation::arming_checks(size_t buflen, char *buffer) const +{ + // throttle runup must be larger than ramp, keep the params up to date to not confuse users + if (bailout_throttle_time.get() + 1 > bailout_runup_time.get()) { + hal.util->snprintf(buffer, buflen, "H_RSC_AROT_RUNUP must be > H_RSC_AROT_RAMP"); + return false; + } + return true; +} diff --git a/libraries/AC_Autorotation/RSC_Autorotation.h b/libraries/AC_Autorotation/RSC_Autorotation.h index a6ae0da63f9a11..8c0bd230a7849f 100644 --- a/libraries/AC_Autorotation/RSC_Autorotation.h +++ b/libraries/AC_Autorotation/RSC_Autorotation.h @@ -33,6 +33,9 @@ class RSC_Autorotation // request changes in autorotation state void set_active(bool active, bool force_state); + // sanity check of parameters, should be called only whilst disarmed + bool arming_checks(size_t buflen, char *buffer) const; + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[];