diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index 576edd7576..ebcb09c6ba 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -46,6 +46,13 @@ const AP_Param::GroupInfo AP_Tuning::var_info[] = { // @Values: 0:Disable,1:Enable // @User: Standard AP_GROUPINFO("MODE_REVERT", 6, AP_Tuning, mode_revert, 1), + + // @Param: ERR_THRESH + // @DisplayName: Controller error threshold + // @Description: This sets the controller error threshold above which an alarm will sound and a message will be sent to the GCS to warn of controller instability + // @Range: 0 1 + // @User: Standard + AP_GROUPINFO("ERR_THRESH", 7, AP_Tuning, error_threshold, 0.15f), AP_GROUPEND }; @@ -176,6 +183,9 @@ void AP_Tuning::check_input(uint8_t flightmode) last_channel_value = chan_value; } + // check for controller error + check_controller_error(); + if (fabsf(chan_value - last_channel_value) < 0.01) { // ignore changes of less than 1% return; @@ -313,3 +323,18 @@ const char *AP_Tuning::get_tuning_name(uint8_t parm) return "UNKNOWN"; } +/* + check for controller error + */ +void AP_Tuning::check_controller_error(void) +{ + float err = controller_error(current_parm); + if (err > error_threshold) { + uint32_t now = AP_HAL::millis(); + if (now - last_controller_error_ms > 2000) { + AP_Notify::events.tune_error = 1; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Tuning: error %.2f", err); + last_controller_error_ms = now; + } + } +} diff --git a/libraries/AP_Tuning/AP_Tuning.h b/libraries/AP_Tuning/AP_Tuning.h index ca6e342b75..59dd7b4c32 100644 --- a/libraries/AP_Tuning/AP_Tuning.h +++ b/libraries/AP_Tuning/AP_Tuning.h @@ -46,6 +46,7 @@ private: AP_Int8 selector; AP_Float range; AP_Int8 mode_revert; + AP_Float error_threshold; // when selector was triggered uint32_t selector_start_ms; @@ -81,6 +82,9 @@ private: // last flight mode we were tuning in uint8_t last_flightmode; + // last time we reported controller error + uint32_t last_controller_error_ms; + const tuning_set *tuning_sets; const tuning_name *tuning_names; @@ -90,6 +94,7 @@ private: void save_parameters(void); void revert_parameters(void); const char *get_tuning_name(uint8_t parm); + void check_controller_error(void); protected: // virtual functions that must be implemented in vehicle subclass @@ -97,6 +102,7 @@ protected: virtual void save_value(uint8_t parm) = 0; virtual void reload_value(uint8_t parm) = 0; virtual void set_value(uint8_t parm, float value) = 0; + virtual float controller_error(uint8_t parm) = 0; // parmset is in vehicle subclass var table AP_Int16 parmset;