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

690 lines
22 KiB
C++
Raw Permalink Blame History

/*
C++ implementation of quicktune based on original lua script
*/
// quicktune is not performance sensitive, save flash
#pragma GCC optimize("Os")
#include "AP_Quicktune.h"
#if AP_QUICKTUNE_ENABLED
#define UPDATE_RATE_HZ 40
#define STAGE_DELAY 4000
#define PILOT_INPUT_DELAY 4000
#define YAW_FLTE_MAX 2.0
#define FLTD_MUL 0.5
#define FLTT_MUL 0.5
#define DEFAULT_SMAX 50.0
#define OPTIONS_TWO_POSITION (1<<0)
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Arming/AP_Arming.h>
#include <GCS_MAVLink/GCS.h>
const AP_Param::GroupInfo AP_Quicktune::var_info[] = {
// @Param: ENABLE
// @DisplayName: Quicktune enable
// @Description: Enable quicktune system
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Quicktune, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: AXES
// @DisplayName: Quicktune axes
// @Description: Axes to tune
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
// @User: Standard
AP_GROUPINFO("AXES", 2, AP_Quicktune, axes_enabled, 7),
// @Param: DOUBLE_TIME
// @DisplayName: Quicktune doubling time
// @Description: Time to double a tuning parameter. Raise this for a slower tune.
// @Range: 5 20
// @Units: s
// @User: Standard
AP_GROUPINFO("DOUBLE_TIME", 3, AP_Quicktune, double_time, 10),
// @Param: GAIN_MARGIN
// @DisplayName: Quicktune gain margin
// @Description: Reduction in gain after oscillation detected. Raise this number to get a more conservative tune
// @Range: 20 80
// @Units: %
// @User: Standard
AP_GROUPINFO("GAIN_MARGIN", 4, AP_Quicktune, gain_margin, 60),
// @Param: OSC_SMAX
// @DisplayName: Quicktune oscillation rate threshold
// @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune.
// @Range: 1 10
// @User: Standard
AP_GROUPINFO("OSC_SMAX", 5, AP_Quicktune, osc_smax, 4),
// @Param: YAW_P_MAX
// @DisplayName: Quicktune Yaw P max
// @Description: Maximum value for yaw P gain
// @Range: 0.1 3
// @User: Standard
AP_GROUPINFO("YAW_P_MAX", 6, AP_Quicktune, yaw_p_max, 0.5),
// @Param: YAW_D_MAX
// @DisplayName: Quicktune Yaw D max
// @Description: Maximum value for yaw D gain
// @Range: 0.001 1
// @User: Standard
AP_GROUPINFO("YAW_D_MAX", 7, AP_Quicktune, yaw_d_max, 0.01),
// @Param: RP_PI_RATIO
// @DisplayName: Quicktune roll/pitch PI ratio
// @Description: Ratio between P and I gains for roll and pitch. Raise this to get a lower I gain
// @Range: 1.0 2.0
// @User: Standard
AP_GROUPINFO("RP_PI_RATIO", 8, AP_Quicktune, rp_pi_ratio, 1.0),
// @Param: Y_PI_RATIO
// @DisplayName: Quicktune Yaw PI ratio
// @Description: Ratio between P and I gains for yaw. Raise this to get a lower I gain
// @Range: 1.0 20
// @User: Standard
AP_GROUPINFO("Y_PI_RATIO", 9, AP_Quicktune, y_pi_ratio, 10),
// @Param: AUTO_FILTER
// @DisplayName: Quicktune auto filter enable
// @Description: When enabled the PID filter settings are automatically set based on INS_GYRO_FILTER
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("AUTO_FILTER", 10, AP_Quicktune, auto_filter, 1),
// @Param: AUTO_SAVE
// @DisplayName: Quicktune auto save
// @Description: Number of seconds after completion of tune to auto-save. This is useful when using a 2 position switch for quicktune. Zero (the default value) disables automatic saving, and you will need to have a 3 position switch to save or use GCS auxilliary functions.
// @Units: s
// @User: Standard
AP_GROUPINFO("AUTO_SAVE", 11, AP_Quicktune, auto_save, 0),
// @Param: REDUCE_MAX
// @DisplayName: Quicktune maximum gain reduction
// @Description: This controls how much quicktune is allowed to lower gains from the original gains. If the vehicle already has a reasonable tune and is not oscillating then you can set this to zero to prevent gain reductions. The default of 20% is reasonable for most vehicles. Using a maximum gain reduction lowers the chance of an angle P oscillation happening if quicktune gets a false positive oscillation at a low gain, which can result in very low rate gains and a dangerous angle P oscillation.
// @Units: %
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("REDUCE_MAX", 12, AP_Quicktune, reduce_max, 20),
// @Param: OPTIONS
// @DisplayName: Quicktune options
// @Description: Additional options. When the Two Position Switch option is enabled then a high switch position will start the tune, low will disable the tune. you should also set a QUIK_AUTO_SAVE time so that you will be able to save the tune.
// @Bitmask: 0:UseTwoPositionSwitch
// @User: Standard
AP_GROUPINFO("OPTIONS", 13, AP_Quicktune, options, 0),
// @Param: ANGLE_MAX
// @DisplayName: maximum angle error for tune abort
// @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Quicktune: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QWIK_DOUBLE_TIME to do the tune more slowly.
// @Units: deg
// @User: Standard
AP_GROUPINFO("ANGLE_MAX", 14, AP_Quicktune, angle_max, 10),
AP_GROUPEND
};
// Call at loop rate
void AP_Quicktune::update(bool mode_supports_quicktune)
{
if (enable < 1) {
if (need_restore) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "QuickTune disabled");
abort_tune();
}
return;
}
const uint32_t now = AP_HAL::millis();
if (!mode_supports_quicktune) {
/*
user has switched to a non-quicktune mode. If we have
pending parameter changes then revert
*/
if (need_restore) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "QuickTune aborted");
}
abort_tune();
return;
}
if (need_restore) {
const float att_error = AC_AttitudeControl::get_singleton()->get_att_error_angle_deg();
if (angle_max > 0 && att_error > angle_max) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Quicktune: attitude error %.1fdeg - ABORTING", att_error);
abort_tune();
return;
}
}
if (have_pilot_input()) {
last_pilot_input_ms = now;
}
SwitchPos sw_pos_tune = SwitchPos::MID;
SwitchPos sw_pos_save = SwitchPos::HIGH;
if ((options & OPTIONS_TWO_POSITION) != 0) {
sw_pos_tune = SwitchPos::HIGH;
sw_pos_save = SwitchPos::NONE;
}
const auto &vehicle = *AP::vehicle();
if (sw_pos == sw_pos_tune &&
(!hal.util->get_soft_armed() || !vehicle.get_likely_flying())) {
if (now - last_warning_ms > 5000) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Must be flying to tune");
last_warning_ms = now;
}
return;
}
if (sw_pos == SwitchPos::LOW || !hal.util->get_soft_armed() || !vehicle.get_likely_flying()) {
// Abort, revert parameters
if (need_restore) {
need_restore = false;
restore_all_params();
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Reverted");
tune_done_time_ms = 0;
}
reset_axes_done();
return;
}
if (sw_pos == sw_pos_save) {
// Save all params
if (need_restore) {
need_restore = false;
save_all_params();
}
}
if (sw_pos != sw_pos_tune) {
return;
}
if (now - last_stage_change_ms < STAGE_DELAY) {
// Update slew gain
if (slew_parm != Param::END) {
const float P = get_param_value(slew_parm);
const AxisName axis = get_axis(slew_parm);
adjust_gain(slew_parm, P+slew_delta);
slew_steps = slew_steps - 1;
Write_QWIK(get_slew_rate(axis), P, slew_parm);
if (slew_steps == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f", get_param_name(slew_parm), P);
slew_parm = Param::END;
if (get_current_axis() == AxisName::DONE) {
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: DONE");
tune_done_time_ms = now;
}
}
}
return;
}
const AxisName axis = get_current_axis();
if (axis == AxisName::DONE) {
// Nothing left to do, check autosave time
if (tune_done_time_ms != 0 && auto_save > 0) {
if (now - tune_done_time_ms > (auto_save*1000)) {
need_restore = false;
save_all_params();
tune_done_time_ms = 0;
}
}
return;
}
if (!need_restore) {
need_restore = true;
// We are just starting tuning, get current values
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Starting tune");
// Get all params
for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
param_saved[p] = get_param_value(Param(p));
}
// Set up SMAX
const Param is[3] { Param::RLL_SMAX, Param::PIT_SMAX, Param::YAW_SMAX };
for (const auto p : is) {
const float smax = get_param_value(p);
if (smax <= 0) {
adjust_gain(p, DEFAULT_SMAX);
}
}
}
if (now - last_pilot_input_ms < PILOT_INPUT_DELAY) {
return;
}
if (!BIT_IS_SET(filters_done, uint8_t(axis))) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting %s tune", get_axis_name(axis));
setup_filters(axis);
}
const Param pname = get_pname(axis, current_stage);
const float pval = get_param_value(pname);
const float limit = gain_limit(pname);
const bool limited = (limit > 0.0 && pval >= limit);
const float srate = get_slew_rate(axis);
const bool oscillating = srate > osc_smax;
// Check if reached limit
if (limited || oscillating) {
float reduction = (100.0-gain_margin)*0.01;
if (!oscillating) {
reduction = 1.0;
}
float new_gain = pval * reduction;
if (limit > 0.0 && new_gain > limit) {
new_gain = limit;
}
float old_gain = param_saved[uint8_t(pname)];
if (new_gain < old_gain && (pname == Param::PIT_D || pname == Param::RLL_D)) {
// We are lowering a D gain from the original gain. Also
// lower the P gain by the same amount so that we don't
// trigger P oscillation. We don't drop P by more than a
// factor of 2
const float ratio = fmaxf(new_gain / old_gain, 0.5);
const uint8_t P_TO_D_OFS = uint8_t(Param::RLL_D) - uint8_t(Param::RLL_P);
const Param P_name = Param(uint8_t(pname)-P_TO_D_OFS); //from D to P
const float old_pval = get_param_value(P_name);;
const float new_pval = old_pval * ratio;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Adjusting %s %.3f -> %.3f", get_param_name(P_name), old_pval, new_pval);
adjust_gain_limited(P_name, new_pval);
}
// Set up slew gain
slew_parm = pname;
const float slew_target = limit_gain(pname, new_gain);
slew_steps = UPDATE_RATE_HZ/2;
slew_delta = (slew_target - get_param_value(pname)) / slew_steps;
Write_QWIK(srate, pval, pname);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Quicktune: %s done", get_param_name(pname));
advance_stage(axis);
last_stage_change_ms = now;
} else {
float new_gain = pval*get_gain_mul();
// cope with the gain starting at zero (some users with have a zero D gain)
new_gain = MAX(new_gain, 0.0001);
adjust_gain_limited(pname, new_gain);
Write_QWIK(srate, pval, pname);
if (now - last_gain_report_ms > 3000U) {
last_gain_report_ms = now;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f sr:%.2f", get_param_name(pname), new_gain, srate);
}
}
}
/*
abort the tune if it has started
*/
void AP_Quicktune::abort_tune()
{
if (need_restore) {
need_restore = false;
restore_all_params();
}
tune_done_time_ms = 0;
reset_axes_done();
sw_pos = SwitchPos::LOW;
}
void AP_Quicktune::update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag)
{
sw_pos = SwitchPos(ch_flag);
}
void AP_Quicktune::reset_axes_done()
{
axes_done = 0;
filters_done = 0;
current_stage = Stage::D;
}
void AP_Quicktune::setup_filters(AP_Quicktune::AxisName axis)
{
if (auto_filter <= 0) {
BIT_SET(filters_done, uint8_t(axis));
return;
}
AP_InertialSensor *imu = AP_InertialSensor::get_singleton();
if (imu == nullptr) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return;
}
const float gyro_filter = imu->get_gyro_filter_hz();
adjust_gain(get_pname(axis, Stage::FLTT), gyro_filter * FLTT_MUL);
adjust_gain(get_pname(axis, Stage::FLTD), gyro_filter * FLTD_MUL);
if (axis == AxisName::YAW) {
const float FLTE = get_param_value(Param::YAW_FLTE);
if (FLTE < 0.0 || FLTE > YAW_FLTE_MAX) {
adjust_gain(Param::YAW_FLTE, YAW_FLTE_MAX);
}
}
BIT_SET(filters_done, uint8_t(axis));
}
// Check for pilot input to pause tune
bool AP_Quicktune::have_pilot_input() const
{
auto &RC = rc();
const float roll = RC.get_roll_channel().norm_input_dz();
const float pitch = RC.get_pitch_channel().norm_input_dz();
const float yaw = RC.get_yaw_channel().norm_input_dz();
if (fabsf(roll) > 0 || fabsf(pitch) > 0 || fabsf(yaw) > 0) {
return true;
}
return false;
}
// Get the axis name we are working on, or DONE for all done
AP_Quicktune::AxisName AP_Quicktune::get_current_axis() const
{
for (int8_t i = 0; i < int8_t(AxisName::DONE); i++) {
if (BIT_IS_SET(axes_enabled, i) && !BIT_IS_SET(axes_done, i)) {
return AxisName(i);
}
}
return AxisName::DONE;
}
// get the AC_PID object for an axis
AC_PID *AP_Quicktune::get_axis_pid(AP_Quicktune::AxisName axis) const
{
auto &attitude_control = *AC_AttitudeControl::get_singleton();
switch(axis) {
case AxisName::RLL:
return &attitude_control.get_rate_roll_pid();
case AxisName::PIT:
return &attitude_control.get_rate_pitch_pid();
case AxisName::YAW:
return &attitude_control.get_rate_yaw_pid();
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
return nullptr;
}
// get slew rate parameter for an axis
float AP_Quicktune::get_slew_rate(AP_Quicktune::AxisName axis) const
{
auto *pid = get_axis_pid(axis);
if (pid == nullptr) {
return 0;
}
return pid->get_pid_info().slew_rate;
}
// Move to next stage of tune
void AP_Quicktune::advance_stage(AP_Quicktune::AxisName axis)
{
if (current_stage == Stage::D) {
current_stage = Stage::P;
} else {
BIT_SET(axes_done, uint8_t(axis));
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: %s done", get_axis_name(axis));
current_stage = Stage::D;
}
}
void AP_Quicktune::adjust_gain(AP_Quicktune::Param param, float value)
{
need_restore = true;
BIT_SET(param_changed, uint8_t(param));
set_param_value(param, value);
if (get_stage(param) == Stage::P) {
// Also change I gain
const uint8_t P_TO_I_OFS = uint8_t(Param::RLL_I) - uint8_t(Param::RLL_P);
const uint8_t P_TO_FF_OFS = uint8_t(Param::RLL_FF) - uint8_t(Param::RLL_P);
const Param iname = Param(uint8_t(param)+P_TO_I_OFS);
const Param ffname = Param(uint8_t(param)+P_TO_FF_OFS);
float FF = get_param_value(ffname);
if (FF > 0) {
// If we have any FF on an axis then we don't couple I to P,
// usually we want I = FF for a one second time constant for trim
return;
}
BIT_SET(param_changed, uint8_t(iname));
// Work out ratio of P to I that we want
float pi_ratio = rp_pi_ratio;
if (get_axis(param) == AxisName::YAW) {
pi_ratio = y_pi_ratio;
}
if (pi_ratio >= 1) {
set_param_value(iname, value/pi_ratio);
}
}
}
void AP_Quicktune::adjust_gain_limited(AP_Quicktune::Param param, float value)
{
adjust_gain(param, limit_gain(param, value));
}
float AP_Quicktune::limit_gain(AP_Quicktune::Param param, float value)
{
const float saved_value = param_saved[uint8_t(param)];
if (reduce_max >= 0 && reduce_max < 100 && saved_value > 0) {
// Check if we exceeded gain reduction
const float reduction_pct = 100.0 * (saved_value - value) / saved_value;
if (reduction_pct > reduce_max) {
const float new_value = saved_value * (100 - reduce_max) * 0.01;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Limiting %s %.3f -> %.3f", get_param_name(param), value, new_value);
value = new_value;
}
}
return value;
}
const char* AP_Quicktune::get_param_name(AP_Quicktune::Param param) const
{
switch (param)
{
case Param::RLL_P:
return "Roll P";
case Param::RLL_I:
return "Roll I";
case Param::RLL_D:
return "Roll D";
case Param::PIT_P:
return "Pitch P";
case Param::PIT_I:
return "Pitch I";
case Param::PIT_D:
return "Pitch D";
case Param::YAW_P:
return "Yaw P";
case Param::YAW_I:
return "Yaw I";
case Param::YAW_D:
return "Yaw D";
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return "UNK";
}
}
float AP_Quicktune::get_gain_mul() const
{
return expf(logf(2.0)/(UPDATE_RATE_HZ*MAX(1,double_time)));
}
void AP_Quicktune::restore_all_params()
{
for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
const auto param = Param(p);
if (BIT_IS_SET(param_changed, p)) {
set_param_value(param, param_saved[p]);
BIT_CLEAR(param_changed, p);
}
}
}
void AP_Quicktune::save_all_params()
{
for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
const auto param = Param(p);
set_and_save_param_value(param, get_param_value(param));
param_saved[p] = get_param_value(param);
BIT_CLEAR(param_changed, p);
}
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Saved");
}
AP_Quicktune::Param AP_Quicktune::get_pname(AP_Quicktune::AxisName axis, AP_Quicktune::Stage stage) const
{
const uint8_t axis_offset = uint8_t(axis) * param_per_axis;
switch (stage)
{
case Stage::P:
return Param(uint8_t(Param::RLL_P) + axis_offset);
case Stage::D:
return Param(uint8_t(Param::RLL_D) + axis_offset);
case Stage::FLTT:
return Param(uint8_t(Param::RLL_FLTT) + axis_offset);
case Stage::FLTD:
return Param(uint8_t(Param::RLL_FLTD) + axis_offset);
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return Param::END;
}
}
AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) const
{
if (param == Param::RLL_P || param == Param::PIT_P || param == Param::YAW_P) {
return Stage::P;
} else {
return Stage::END; //Means "anything but P gain"
}
}
AP_Float *AP_Quicktune::get_param_pointer(AP_Quicktune::Param param) const
{
const AxisName axis = get_axis(param);
auto *pid = get_axis_pid(axis);
if (pid == nullptr) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return nullptr;
}
const Param roll_param = Param(uint8_t(param) % param_per_axis);
switch (roll_param)
{
case Param::RLL_P:
return &pid->kP();
case Param::RLL_I:
return &pid->kI();
case Param::RLL_D:
return &pid->kD();
case Param::RLL_SMAX:
return &pid->slew_limit();
case Param::RLL_FLTT:
return &pid->filt_T_hz();
case Param::RLL_FLTD:
return &pid->filt_D_hz();
case Param::RLL_FLTE:
return &pid->filt_E_hz();
case Param::RLL_FF:
return &pid->ff();
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return nullptr;
}
}
float AP_Quicktune::get_param_value(AP_Quicktune::Param param) const
{
AP_Float *ptr = get_param_pointer(param);
if (ptr != nullptr) {
return ptr->get();
}
return 0.0;
}
void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value)
{
AP_Float *ptr = get_param_pointer(param);
if (ptr != nullptr) {
ptr->set(value);
}
}
void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value)
{
AP_Float *ptr = get_param_pointer(param);
if (ptr != nullptr) {
ptr->set_and_save(value);
}
}
AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) const
{
if (param >= Param::END) {
return AxisName::END;
}
return AxisName(uint8_t(param) / param_per_axis);
}
const char* AP_Quicktune::get_axis_name(AP_Quicktune::AxisName axis) const
{
switch (axis)
{
case AxisName::RLL:
return "Roll";
case AxisName::PIT:
return "Pitch";
case AxisName::YAW:
return "Yaw";
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return "UNK";
}
}
float AP_Quicktune::gain_limit(AP_Quicktune::Param param) const
{
if (get_axis(param) == AxisName::YAW) {
if (param == Param::YAW_P) {
return yaw_p_max;
}
if (param == Param::YAW_D) {
return yaw_d_max;
}
}
return 0.0;
}
// @LoggerMessage: QWIK
// @Description: Quicktune
// @Field: TimeUS: Time since system startup
// @Field: ParamNo: number of parameter being tuned
// @Field: SRate: slew rate
// @Field: Gain: test gain for current axis and PID element
// @Field: Param: name of parameter being being tuned
void AP_Quicktune::Write_QWIK(float srate, float gain, AP_Quicktune::Param param)
{
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("QWIK","TimeUS,ParamNo,SRate,Gain,Param", "s#---", "-----", "QBffN",
AP_HAL::micros64(),
unsigned(param),
srate,
gain,
get_param_name(param));
#endif
}
#endif //AP_QUICKTUNE_ENABLED
Reference in New Issue View Git Blame Copy Permalink