mirror of https://github.com/ArduPilot/ardupilot
AP_Quicktune: Added Quicktune
C++ version of lua script, with some enhancements
This commit is contained in:
Michelle Rossouw
Andrew Tridgell
parent
316bfd3263
commit
edcddcb82e
|
|
@ -0,0 +1,689 @@
|
|||
/*
|
||||
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, 5),
|
||||
|
||||
// @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
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ANGLE_MAX", 14, AP_Quicktune, angle_max, 15),
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,167 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
#ifndef AP_QUICKTUNE_ENABLED
|
||||
#define AP_QUICKTUNE_ENABLED 1 // NOTE: may be disabled by vehicle header
|
||||
#endif
|
||||
|
||||
#if AP_QUICKTUNE_ENABLED
|
||||
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
|
||||
class AP_Quicktune {
|
||||
public:
|
||||
AP_Quicktune()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
// Empty destructor to suppress compiler warning
|
||||
virtual ~AP_Quicktune() {}
|
||||
|
||||
/* Do not allow copies */
|
||||
CLASS_NO_COPY(AP_Quicktune);
|
||||
|
||||
// Parameter block
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
void update(bool mode_supports_quicktune);
|
||||
void update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag);
|
||||
|
||||
private:
|
||||
|
||||
// Parameters
|
||||
AP_Int8 enable;
|
||||
AP_Int8 axes_enabled;
|
||||
AP_Float double_time;
|
||||
AP_Float gain_margin;
|
||||
AP_Float osc_smax;
|
||||
AP_Float yaw_p_max;
|
||||
AP_Float yaw_d_max;
|
||||
AP_Float rp_pi_ratio;
|
||||
AP_Float y_pi_ratio;
|
||||
AP_Int8 auto_filter;
|
||||
AP_Float auto_save;
|
||||
AP_Float reduce_max;
|
||||
AP_Int16 options;
|
||||
AP_Int8 angle_max;
|
||||
|
||||
// Low, Mid and High must be in the same positions as they are in RC_Channel::AuxSwitchPos
|
||||
enum class SwitchPos : uint8_t {
|
||||
LOW,
|
||||
MID,
|
||||
HIGH,
|
||||
NONE,
|
||||
};
|
||||
|
||||
|
||||
enum class AxisName : uint8_t {
|
||||
RLL = 0,
|
||||
PIT,
|
||||
YAW,
|
||||
DONE,
|
||||
END,
|
||||
};
|
||||
|
||||
/*
|
||||
note! we rely on the enum being in the same order between axes
|
||||
*/
|
||||
enum class Param : uint8_t {
|
||||
RLL_P = 0,
|
||||
RLL_I,
|
||||
RLL_D,
|
||||
RLL_SMAX,
|
||||
RLL_FLTT,
|
||||
RLL_FLTD,
|
||||
RLL_FLTE,
|
||||
RLL_FF,
|
||||
|
||||
PIT_P,
|
||||
PIT_I,
|
||||
PIT_D,
|
||||
PIT_SMAX,
|
||||
PIT_FLTT,
|
||||
PIT_FLTD,
|
||||
PIT_FLTE,
|
||||
PIT_FF,
|
||||
|
||||
YAW_P,
|
||||
YAW_I,
|
||||
YAW_D,
|
||||
YAW_SMAX,
|
||||
YAW_FLTT,
|
||||
YAW_FLTD,
|
||||
YAW_FLTE,
|
||||
YAW_FF,
|
||||
END,
|
||||
};
|
||||
|
||||
static const uint8_t param_per_axis = uint8_t(Param::PIT_P) - uint8_t(Param::RLL_P);
|
||||
static_assert(uint8_t(Param::END) == 3*param_per_axis, "AP_Quicktune Param error");
|
||||
|
||||
// Also the gains
|
||||
enum class Stage : uint8_t {
|
||||
D,
|
||||
P,
|
||||
DONE,
|
||||
I,
|
||||
FF,
|
||||
SMAX,
|
||||
FLTT,
|
||||
FLTD,
|
||||
FLTE,
|
||||
END,
|
||||
};
|
||||
|
||||
// Time keeping
|
||||
uint32_t last_stage_change_ms;
|
||||
uint32_t last_gain_report_ms;
|
||||
uint32_t last_pilot_input_ms;
|
||||
uint32_t last_warning_ms;
|
||||
uint32_t tune_done_time_ms;
|
||||
|
||||
// Bitmasks
|
||||
uint32_t axes_done;
|
||||
uint32_t filters_done;
|
||||
uint32_t param_changed; //Bitmask of changed parameters
|
||||
|
||||
Stage current_stage = Stage::D;
|
||||
Param slew_parm = Param::END;
|
||||
uint8_t slew_steps;
|
||||
float slew_delta;
|
||||
SwitchPos sw_pos; //Switch pos to be set by aux func
|
||||
bool need_restore;
|
||||
float param_saved[uint8_t(Param::END)]; //Saved values of the parameters
|
||||
|
||||
void reset_axes_done();
|
||||
void setup_filters(AxisName axis);
|
||||
bool have_pilot_input() const;
|
||||
AxisName get_current_axis() const;
|
||||
float get_slew_rate(AxisName axis) const;
|
||||
void advance_stage(AxisName axis);
|
||||
void adjust_gain(Param param, float value);
|
||||
void adjust_gain_limited(Param param, float value);
|
||||
float get_gain_mul() const;
|
||||
void restore_all_params();
|
||||
void save_all_params();
|
||||
Param get_pname(AxisName axis, Stage stage) const;
|
||||
AP_Float *get_param_pointer(Param param) const;
|
||||
float get_param_value(Param param) const;
|
||||
void set_param_value(Param param, float value);
|
||||
void set_and_save_param_value(Param param, float value);
|
||||
float gain_limit(Param param) const;
|
||||
AxisName get_axis(Param param) const;
|
||||
float limit_gain(Param param, float value);
|
||||
const char* get_param_name(Param param) const;
|
||||
Stage get_stage(Param param) const;
|
||||
const char* get_axis_name(AxisName axis) const;
|
||||
AC_PID *get_axis_pid(AxisName axis) const;
|
||||
void Write_QWIK(float SRate, float Gain, Param param);
|
||||
|
||||
void abort_tune(void);
|
||||
};
|
||||
|
||||
#endif // AP_QUICKTUNE_ENABLED
|
||||
Loading…
Reference in New Issue