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

AP_Tuning: added monitoring of controller output

This commit is contained in:
Andrew Tridgell 2016-05-27 14:13:29 +10:00
parent 6330060e49
commit 006aafab3f
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Tuning/AP_Tuning.cpp
View File

@ -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;
}
}
}

6
libraries/AP_Tuning/AP_Tuning.h
View File

@ -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;