mirror of https://github.com/ArduPilot/ardupilot
APM_Control: implement new autotune system
use actuator/rate ratio with median filter for FF and Dmod/overshoot for P,I,D
This commit is contained in:
Andrew Tridgell
parent
12fad55891
commit
f88622def8
|
|
@ -43,28 +43,26 @@ extern const AP_HAL::HAL& hal;
|
|||
// time in milliseconds between autotune saves
|
||||
#define AUTOTUNE_SAVE_PERIOD 10000U
|
||||
|
||||
// how much time we have to overshoot for to reduce gains
|
||||
#define AUTOTUNE_OVERSHOOT_TIME 100
|
||||
|
||||
// how much time we have to undershoot for to increase gains
|
||||
#define AUTOTUNE_UNDERSHOOT_TIME 200
|
||||
|
||||
// step size for increasing gains, percentage
|
||||
#define AUTOTUNE_INCREASE_STEP 5
|
||||
#define AUTOTUNE_INCREASE_FF_STEP 12
|
||||
#define AUTOTUNE_INCREASE_PD_STEP 5
|
||||
|
||||
// step size for increasing gains when low impact, percentage
|
||||
#define AUTOTUNE_INCREASE_PD_LOW_STEP 30
|
||||
|
||||
// step size for decreasing gains, percentage
|
||||
#define AUTOTUNE_DECREASE_STEP 8
|
||||
#define AUTOTUNE_DECREASE_FF_STEP 15
|
||||
#define AUTOTUNE_DECREASE_PD_STEP 20
|
||||
|
||||
// min/max FF gains
|
||||
#define AUTOTUNE_MAX_FF 2.0f
|
||||
#define AUTOTUNE_MIN_FF 0.05f
|
||||
// limits on IMAX
|
||||
#define AUTOTUNE_MIN_IMAX 0.4
|
||||
#define AUTOTUNE_MAX_IMAX 0.9
|
||||
|
||||
// tau ranges
|
||||
#define AUTOTUNE_MAX_TAU 0.7f
|
||||
#define AUTOTUNE_MIN_TAU 0.2f
|
||||
// ratio of I to P
|
||||
#define AUTOTUNE_I_RATIO 0.75
|
||||
|
||||
#define AUTOTUNE_MIN_IMAX 2000
|
||||
#define AUTOTUNE_MAX_IMAX 4000
|
||||
// overshoot threshold
|
||||
#define AUTOTUNE_OVERSHOOT 1.1
|
||||
|
||||
// constructor
|
||||
AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
|
||||
|
|
@ -72,10 +70,9 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
|
|||
AC_PID &_rpid) :
|
||||
current(_gains),
|
||||
rpid(_rpid),
|
||||
running(false),
|
||||
type(_type),
|
||||
aparm(parms),
|
||||
saturated_surfaces(false)
|
||||
ff_filter(2)
|
||||
{}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
|
@ -93,20 +90,19 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
|
|||
*/
|
||||
static const struct {
|
||||
float tau;
|
||||
float Pratio;
|
||||
float rmax;
|
||||
} tuning_table[] = {
|
||||
{ 0.70f, 0.050f, 20 }, // level 1
|
||||
{ 0.65f, 0.055f, 30 }, // level 2
|
||||
{ 0.60f, 0.060f, 40 }, // level 3
|
||||
{ 0.55f, 0.065f, 50 }, // level 4
|
||||
{ 0.50f, 0.070f, 60 }, // level 5
|
||||
{ 0.45f, 0.075f, 75 }, // level 6
|
||||
{ 0.40f, 0.080f, 90 }, // level 7
|
||||
{ 0.30f, 0.085f, 120 }, // level 8
|
||||
{ 0.20f, 0.090f, 160 }, // level 9
|
||||
{ 0.10f, 0.095f, 210 }, // level 10
|
||||
{ 0.05f, 0.100f, 300 }, // (yes, it goes to 11)
|
||||
{ 1.00, 20 }, // level 1
|
||||
{ 0.90, 30 }, // level 2
|
||||
{ 0.80, 40 }, // level 3
|
||||
{ 0.70, 50 }, // level 4
|
||||
{ 0.60, 60 }, // level 5
|
||||
{ 0.50, 75 }, // level 6
|
||||
{ 0.25, 90 }, // level 7
|
||||
{ 0.15, 120 }, // level 8
|
||||
{ 0.10, 160 }, // level 9
|
||||
{ 0.05, 210 }, // level 10
|
||||
{ 0.03, 300 }, // (yes, it goes to 11)
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
@ -115,14 +111,12 @@ static const struct {
|
|||
void AP_AutoTune::start(void)
|
||||
{
|
||||
running = true;
|
||||
state = DEMAND_UNSATURATED;
|
||||
state = ATState::IDLE;
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
state_enter_ms = now;
|
||||
last_save_ms = now;
|
||||
|
||||
last_save = current;
|
||||
restore = current;
|
||||
restore = last_save = get_gains(current);
|
||||
|
||||
uint8_t level = aparm.autotune_level;
|
||||
if (level > ARRAY_SIZE(tuning_table)) {
|
||||
|
|
@ -132,13 +126,20 @@ void AP_AutoTune::start(void)
|
|||
level = 1;
|
||||
}
|
||||
|
||||
current.rmax.set(tuning_table[level-1].rmax);
|
||||
current.rmax_pos.set(tuning_table[level-1].rmax);
|
||||
current.rmax_neg.set(tuning_table[level-1].rmax);
|
||||
current.tau.set(tuning_table[level-1].tau);
|
||||
|
||||
current.imax = constrain_float(current.imax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX);
|
||||
rpid.kIMAX().set(constrain_float(rpid.kIMAX(), AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX));
|
||||
|
||||
next_save = current;
|
||||
|
||||
// use 2Hz filters on the actuator and rate to reduce impact of noise
|
||||
actuator_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
|
||||
rate_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
|
||||
|
||||
// scale slew limit to more agressively find oscillations during autotune
|
||||
rpid.set_slew_limit_scale(1.5*45);
|
||||
|
||||
Debug("START FF -> %.3f\n", rpid.ff().get());
|
||||
}
|
||||
|
||||
|
|
@ -150,82 +151,213 @@ void AP_AutoTune::stop(void)
|
|||
if (running) {
|
||||
running = false;
|
||||
save_gains(restore);
|
||||
rpid.set_slew_limit_scale(45);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// @LoggerMessage: ATNP
|
||||
// @Description: Plane AutoTune
|
||||
// @Vehicles: Plane
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: Axis: which axis is currently being tuned
|
||||
// @Field: State: tuning state
|
||||
// @Field: Sur: control surface deflection
|
||||
// @Field: Tar: target rate
|
||||
// @Field: FF0: FF value single sample
|
||||
// @Field: FF: FF value
|
||||
// @Field: P: P value
|
||||
// @Field: D: D value
|
||||
// @Field: Action: action taken
|
||||
|
||||
/*
|
||||
one update cycle of the autotuner
|
||||
*/
|
||||
void AP_AutoTune::update(float desired_rate, float achieved_rate, float servo_out)
|
||||
void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||
{
|
||||
if (!running) {
|
||||
return;
|
||||
}
|
||||
check_save();
|
||||
|
||||
// see what state we are in
|
||||
ATState new_state;
|
||||
float abs_desired_rate = fabsf(desired_rate);
|
||||
uint32_t now = AP_HAL::millis();
|
||||
ATState new_state = state;
|
||||
const float desired_rate = pinfo.target;
|
||||
// filter actuator without I term
|
||||
const float actuator = actuator_filter.apply(pinfo.FF + pinfo.P + pinfo.D);
|
||||
const float actual_rate = rate_filter.apply(pinfo.actual);
|
||||
|
||||
if (fabsf(servo_out) >= 45) {
|
||||
// we have saturated the servo demand (not including
|
||||
// integrator), we cannot get any information that would allow
|
||||
// us to increase the gain
|
||||
saturated_surfaces = true;
|
||||
}
|
||||
max_actuator = MAX(max_actuator, actuator);
|
||||
min_actuator = MIN(min_actuator, actuator);
|
||||
max_rate = MAX(max_rate, actual_rate);
|
||||
min_rate = MIN(min_rate, actual_rate);
|
||||
max_target = MAX(max_target, desired_rate);
|
||||
min_target = MIN(min_target, desired_rate);
|
||||
max_P = MAX(max_P, fabsf(pinfo.P));
|
||||
max_D = MAX(max_D, fabsf(pinfo.D));
|
||||
min_Dmod = MIN(min_Dmod, pinfo.Dmod);
|
||||
|
||||
if (abs_desired_rate < 0.8f * current.rmax) {
|
||||
// we are not demanding max rate
|
||||
new_state = DEMAND_UNSATURATED;
|
||||
} else if (fabsf(achieved_rate) > abs_desired_rate) {
|
||||
new_state = desired_rate > 0 ? DEMAND_OVER_POS : DEMAND_OVER_NEG;
|
||||
int16_t att_limit_cd;
|
||||
if (type == AUTOTUNE_ROLL) {
|
||||
att_limit_cd = aparm.roll_limit_cd;
|
||||
} else {
|
||||
new_state = desired_rate > 0 ? DEMAND_UNDER_POS : DEMAND_UNDER_NEG;
|
||||
att_limit_cd = MIN(abs(aparm.pitch_limit_max_cd),abs(aparm.pitch_limit_min_cd));
|
||||
}
|
||||
if (new_state != state) {
|
||||
check_state_exit(now - state_enter_ms);
|
||||
state = new_state;
|
||||
state_enter_ms = now;
|
||||
saturated_surfaces = false;
|
||||
}
|
||||
if (state != DEMAND_UNSATURATED) {
|
||||
write_log(servo_out, desired_rate, achieved_rate);
|
||||
}
|
||||
}
|
||||
const float rate_threshold1 = 0.75 * MIN(att_limit_cd * 0.01 / current.tau.get(), current.rmax_pos);
|
||||
const float rate_threshold2 = 0.25 * rate_threshold1;
|
||||
|
||||
/*
|
||||
called when we change state to see if we should change gains
|
||||
*/
|
||||
void AP_AutoTune::check_state_exit(uint32_t state_time_ms)
|
||||
{
|
||||
switch (state) {
|
||||
case DEMAND_UNSATURATED:
|
||||
break;
|
||||
case DEMAND_UNDER_POS:
|
||||
case DEMAND_UNDER_NEG:
|
||||
// we increase P if we have not saturated the surfaces during
|
||||
// this state, and we have
|
||||
if (state_time_ms >= AUTOTUNE_UNDERSHOOT_TIME && !saturated_surfaces) {
|
||||
rpid.ff().set(rpid.ff() * (100+AUTOTUNE_INCREASE_STEP) * 0.01f);
|
||||
if (rpid.ff() > AUTOTUNE_MAX_FF) {
|
||||
rpid.ff().set(AUTOTUNE_MAX_FF);
|
||||
}
|
||||
Debug("UNDER FF -> %.3f\n", rpid.ff().get());
|
||||
case ATState::IDLE:
|
||||
if (desired_rate > rate_threshold1) {
|
||||
new_state = ATState::DEMAND_POS;
|
||||
} else if (desired_rate < -rate_threshold1) {
|
||||
new_state = ATState::DEMAND_NEG;
|
||||
}
|
||||
break;
|
||||
case DEMAND_OVER_POS:
|
||||
case DEMAND_OVER_NEG:
|
||||
if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) {
|
||||
rpid.ff().set(rpid.ff() * (100-AUTOTUNE_DECREASE_STEP) * 0.01f);
|
||||
if (rpid.ff() < AUTOTUNE_MIN_FF) {
|
||||
rpid.ff().set(AUTOTUNE_MIN_FF);
|
||||
}
|
||||
Debug("OVER FF -> %.3f\n", rpid.ff().get());
|
||||
case ATState::DEMAND_POS:
|
||||
if (desired_rate < rate_threshold2) {
|
||||
new_state = ATState::IDLE;
|
||||
}
|
||||
break;
|
||||
case ATState::DEMAND_NEG:
|
||||
if (desired_rate > -rate_threshold2) {
|
||||
new_state = ATState::IDLE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
AP::logger().Write(
|
||||
type==AUTOTUNE_ROLL?"ATNR":"ATNP",
|
||||
"TimeUS,Axis,State,Sur,Tar,FF0,FF,P,D,Action",
|
||||
"s--dk-----",
|
||||
"F--000000-",
|
||||
"QBBffffffB",
|
||||
AP_HAL::micros64(),
|
||||
unsigned(type),
|
||||
unsigned(new_state),
|
||||
actuator,
|
||||
desired_rate,
|
||||
FF0,
|
||||
current.FF,
|
||||
current.P,
|
||||
current.D,
|
||||
unsigned(action));
|
||||
|
||||
if (new_state == state) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
if (new_state != ATState::IDLE) {
|
||||
// starting an event
|
||||
min_actuator = max_actuator = min_rate = max_rate = 0;
|
||||
state_enter_ms = now;
|
||||
state = new_state;
|
||||
return;
|
||||
}
|
||||
|
||||
if ((state == ATState::DEMAND_POS && max_rate < 0.01 * current.rmax_pos) ||
|
||||
(state == ATState::DEMAND_NEG && min_rate > -0.01 * current.rmax_neg)) {
|
||||
// we didn't get enough rate
|
||||
state = ATState::IDLE;
|
||||
action = Action::LOW_RATE;
|
||||
min_Dmod = 1;
|
||||
max_P = max_D = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (now - state_enter_ms < 100) {
|
||||
// not long enough sample
|
||||
state = ATState::IDLE;
|
||||
action = Action::SHORT;
|
||||
min_Dmod = 1;
|
||||
max_P = max_D = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// we've finished an event. calculate the single-event FF value
|
||||
if (state == ATState::DEMAND_POS) {
|
||||
FF0 = max_actuator / (max_rate * scaler);
|
||||
} else {
|
||||
FF0 = min_actuator / (min_rate * scaler);
|
||||
}
|
||||
|
||||
// apply median filter
|
||||
float FF = ff_filter.apply(FF0);
|
||||
|
||||
const float old_FF = rpid.ff();
|
||||
|
||||
// limit size of change in FF
|
||||
FF = constrain_float(FF,
|
||||
old_FF*(1-AUTOTUNE_DECREASE_FF_STEP*0.01),
|
||||
old_FF*(1+AUTOTUNE_INCREASE_FF_STEP*0.01));
|
||||
|
||||
// did the P or D components go over 15% of total actuator?
|
||||
const float abs_actuator = MAX(max_actuator, fabsf(min_actuator));
|
||||
const float PD_high = 0.15 * abs_actuator;
|
||||
bool PD_significant = (max_P > PD_high || max_D > PD_high);
|
||||
|
||||
// see if we overshot
|
||||
bool overshot = (state == ATState::DEMAND_POS)?
|
||||
(max_rate > max_target*AUTOTUNE_OVERSHOOT):
|
||||
(min_rate < min_target*AUTOTUNE_OVERSHOOT);
|
||||
|
||||
// adjust P and D
|
||||
float D = rpid.kD();
|
||||
float P = rpid.kP();
|
||||
|
||||
D = MAX(D, 0.0005);
|
||||
P = MAX(P, 0.01);
|
||||
|
||||
// if the slew limiter kicked in or
|
||||
if (min_Dmod < 1.0 || (overshot && PD_significant)) {
|
||||
// we're overshooting or oscillating, decrease gains. We
|
||||
// assume the gain that needs to be reduced is the one that
|
||||
// peaked at a higher value
|
||||
if (max_P < max_D) {
|
||||
D *= (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01;
|
||||
} else {
|
||||
P *= (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01;
|
||||
}
|
||||
action = Action::LOWER_PD;
|
||||
} else {
|
||||
const float low_PD = 0.05 * MAX(max_actuator, fabsf(min_actuator));
|
||||
// not oscillating or overshooting, increase the gains
|
||||
if (max_P < low_PD) {
|
||||
// P is very small, increase rapidly
|
||||
P *= (100 + AUTOTUNE_INCREASE_PD_LOW_STEP)*0.01;
|
||||
} else {
|
||||
P *= (100 + AUTOTUNE_INCREASE_PD_STEP)*0.01;
|
||||
}
|
||||
if (max_D < low_PD) {
|
||||
// D is very small, increase rapidly
|
||||
D *= (100 + AUTOTUNE_INCREASE_PD_LOW_STEP)*0.01;
|
||||
} else {
|
||||
D *= (100 + AUTOTUNE_INCREASE_PD_STEP)*0.01;
|
||||
}
|
||||
action = Action::RAISE_PD;
|
||||
}
|
||||
|
||||
|
||||
rpid.ff().set(FF);
|
||||
rpid.kP().set(P);
|
||||
rpid.kD().set(D);
|
||||
rpid.kI().set(P*AUTOTUNE_I_RATIO);
|
||||
current.FF = FF;
|
||||
current.P = P;
|
||||
current.I = rpid.kI().get();
|
||||
current.D = D;
|
||||
|
||||
Debug("FPID=(%.3f, %.3f, %.3f, %.3f)\n",
|
||||
rpid.ff().get(),
|
||||
rpid.kP().get(),
|
||||
rpid.kI().get(),
|
||||
rpid.kD().get());
|
||||
|
||||
min_Dmod = 1;
|
||||
max_P = max_D = 0;
|
||||
state = new_state;
|
||||
state_enter_ms = now;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -241,67 +373,44 @@ void AP_AutoTune::check_save(void)
|
|||
// the last save period. This means the pilot has
|
||||
// AUTOTUNE_SAVE_PERIOD milliseconds to decide they don't like the
|
||||
// gains and switch out of autotune
|
||||
ATGains tmp = current;
|
||||
ATGains tmp = get_gains(current);
|
||||
|
||||
save_gains(next_save);
|
||||
Debug("SAVE FF -> %.3f\n", rpid.ff().get());
|
||||
last_save = next_save;
|
||||
|
||||
// restore our current gains
|
||||
current = tmp;
|
||||
set_gains(tmp);
|
||||
|
||||
// if the pilot exits autotune they get these saved values
|
||||
restore = next_save;
|
||||
|
||||
// the next values to save will be the ones we are flying now
|
||||
next_save = current;
|
||||
next_save = tmp;
|
||||
last_save_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
/*
|
||||
log a parameter change from autotune
|
||||
*/
|
||||
void AP_AutoTune::log_param_change(float v, const char *suffix)
|
||||
{
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
if (!logger->logging_started()) {
|
||||
return;
|
||||
}
|
||||
char key[AP_MAX_NAME_SIZE+1];
|
||||
if (type == AUTOTUNE_ROLL) {
|
||||
strncpy(key, "RLL2SRV_", 9);
|
||||
strncpy(&key[8], suffix, AP_MAX_NAME_SIZE-8);
|
||||
} else {
|
||||
strncpy(key, "PTCH2SRV_", 10);
|
||||
strncpy(&key[9], suffix, AP_MAX_NAME_SIZE-9);
|
||||
}
|
||||
key[AP_MAX_NAME_SIZE] = 0;
|
||||
logger->Write_Parameter(key, v);
|
||||
}
|
||||
|
||||
/*
|
||||
set a float and save a float if it has changed by more than
|
||||
0.1%. This reduces the number of insignificant EEPROM writes
|
||||
*/
|
||||
void AP_AutoTune::save_float_if_changed(AP_Float &v, float value, const char *suffix)
|
||||
void AP_AutoTune::save_float_if_changed(AP_Float &v, float value)
|
||||
{
|
||||
float old_value = v.get();
|
||||
v.set(value);
|
||||
if (value <= 0 || fabsf((value-old_value)/value) > 0.001f) {
|
||||
v.save();
|
||||
log_param_change(v.get(), suffix);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
set a int16 and save if changed
|
||||
*/
|
||||
void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value, const char *suffix)
|
||||
void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value)
|
||||
{
|
||||
int16_t old_value = v.get();
|
||||
v.set(value);
|
||||
if (old_value != v.get()) {
|
||||
v.save();
|
||||
log_param_change(v.get(), suffix);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -311,35 +420,43 @@ void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value, const char *
|
|||
*/
|
||||
void AP_AutoTune::save_gains(const ATGains &v)
|
||||
{
|
||||
ATGains tmp = current;
|
||||
current = last_save;
|
||||
save_float_if_changed(current.tau, v.tau, "TCONST");
|
||||
#if 0
|
||||
save_float_if_changed(rpid.kP(), v.P, "P");
|
||||
save_float_if_changed(rpid.kI(), v.I, "I");
|
||||
save_float_if_changed(rpid.kD(), v.D, "D");
|
||||
save_float_if_changed(rpid.ff(), v.FF, "FF");
|
||||
save_int16_if_changed(current.rmax, v.rmax, "RMAX");
|
||||
save_int16_if_changed(current.imax, v.imax, "IMAX");
|
||||
#endif
|
||||
last_save = current;
|
||||
save_float_if_changed(current.tau, v.tau);
|
||||
save_int16_if_changed(current.rmax_pos, v.rmax_pos);
|
||||
save_int16_if_changed(current.rmax_neg, v.rmax_neg);
|
||||
save_float_if_changed(rpid.ff(), v.FF);
|
||||
save_float_if_changed(rpid.kP(), v.P);
|
||||
save_float_if_changed(rpid.kI(), v.I);
|
||||
save_float_if_changed(rpid.kD(), v.D);
|
||||
save_float_if_changed(rpid.kIMAX(), v.IMAX);
|
||||
last_save = get_gains(current);
|
||||
current = tmp;
|
||||
}
|
||||
|
||||
void AP_AutoTune::write_log(float servo, float demanded, float achieved)
|
||||
/*
|
||||
get gains with PID components
|
||||
*/
|
||||
AP_AutoTune::ATGains AP_AutoTune::get_gains(const ATGains &v)
|
||||
{
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
if (!logger->logging_started()) {
|
||||
return;
|
||||
}
|
||||
|
||||
struct log_ATRP pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
type : static_cast<uint8_t>(type),
|
||||
state : (uint8_t)state,
|
||||
servo : (int16_t)(servo*100),
|
||||
demanded : demanded,
|
||||
achieved : achieved,
|
||||
P : rpid.kP().get()
|
||||
};
|
||||
logger->WriteBlock(&pkt, sizeof(pkt));
|
||||
ATGains ret = v;
|
||||
ret.FF = rpid.ff();
|
||||
ret.P = rpid.kP();
|
||||
ret.I = rpid.kI();
|
||||
ret.D = rpid.kD();
|
||||
ret.IMAX = rpid.kIMAX();
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
set gains with PID components
|
||||
*/
|
||||
void AP_AutoTune::set_gains(const ATGains &v)
|
||||
{
|
||||
current = v;
|
||||
rpid.ff().set(v.FF);
|
||||
rpid.kP().set(v.P);
|
||||
rpid.kI().set(v.I);
|
||||
rpid.kD().set(v.D);
|
||||
rpid.kIMAX().set(v.IMAX);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -10,8 +10,9 @@ class AP_AutoTune {
|
|||
public:
|
||||
struct ATGains {
|
||||
AP_Float tau;
|
||||
AP_Int16 rmax;
|
||||
AP_Int16 imax;
|
||||
AP_Int16 rmax_pos;
|
||||
AP_Int16 rmax_neg;
|
||||
float FF, P, I, D, IMAX;
|
||||
};
|
||||
|
||||
enum ATType {
|
||||
|
|
@ -42,11 +43,11 @@ public:
|
|||
void stop(void);
|
||||
|
||||
// update called whenever autotune mode is active. This is
|
||||
// typically at 50Hz
|
||||
void update(float desired_rate, float achieved_rate, float servo_out);
|
||||
// called at the main loop rate
|
||||
void update(AP_Logger::PID_Info &pid_info, float scaler);
|
||||
|
||||
// are we running?
|
||||
bool running:1;
|
||||
bool running;
|
||||
|
||||
private:
|
||||
// the current gains
|
||||
|
|
@ -58,9 +59,6 @@ private:
|
|||
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
|
||||
// did we saturate surfaces?
|
||||
bool saturated_surfaces:1;
|
||||
|
||||
// values to restore if we leave autotune mode
|
||||
ATGains restore;
|
||||
|
||||
|
|
@ -71,25 +69,50 @@ private:
|
|||
ATGains next_save;
|
||||
|
||||
// time when we last saved
|
||||
uint32_t last_save_ms = 0;
|
||||
uint32_t last_save_ms;
|
||||
|
||||
// the demanded/achieved state
|
||||
enum ATState {DEMAND_UNSATURATED,
|
||||
DEMAND_UNDER_POS,
|
||||
DEMAND_OVER_POS,
|
||||
DEMAND_UNDER_NEG,
|
||||
DEMAND_OVER_NEG} state = DEMAND_UNSATURATED;
|
||||
enum class ATState {IDLE,
|
||||
DEMAND_POS,
|
||||
DEMAND_NEG};
|
||||
ATState state;
|
||||
|
||||
// the demanded/achieved state
|
||||
enum class Action {NONE,
|
||||
LOW_RATE,
|
||||
SHORT,
|
||||
RAISE_PD,
|
||||
LOWER_PD};
|
||||
Action action;
|
||||
|
||||
// when we entered the current state
|
||||
uint32_t state_enter_ms = 0;
|
||||
uint32_t state_enter_ms;
|
||||
|
||||
void check_save(void);
|
||||
void check_state_exit(uint32_t state_time_ms);
|
||||
void save_gains(const ATGains &v);
|
||||
|
||||
void write_log(float servo, float demanded, float achieved);
|
||||
void save_float_if_changed(AP_Float &v, float value);
|
||||
void save_int16_if_changed(AP_Int16 &v, int16_t value);
|
||||
|
||||
void log_param_change(float v, const char *suffix);
|
||||
void save_float_if_changed(AP_Float &v, float value, const char *suffix);
|
||||
void save_int16_if_changed(AP_Int16 &v, int16_t value, const char *suffix);
|
||||
// get gains with PID components
|
||||
ATGains get_gains(const ATGains ¤t);
|
||||
void set_gains(const ATGains &v);
|
||||
|
||||
// 5 point mode filter for FF estimate
|
||||
ModeFilterFloat_Size5 ff_filter;
|
||||
|
||||
LowPassFilterFloat actuator_filter;
|
||||
LowPassFilterFloat rate_filter;
|
||||
|
||||
float max_actuator;
|
||||
float min_actuator;
|
||||
float max_rate;
|
||||
float min_rate;
|
||||
float max_target;
|
||||
float min_target;
|
||||
float max_P;
|
||||
float max_D;
|
||||
float min_Dmod;
|
||||
float FF0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
|
|||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_RMAX_UP", 4, AP_PitchController, gains.rmax, 0.0f),
|
||||
AP_GROUPINFO("2SRV_RMAX_UP", 4, AP_PitchController, gains.rmax_pos, 0.0f),
|
||||
|
||||
// @Param: 2SRV_RMAX_DN
|
||||
// @DisplayName: Pitch down max rate
|
||||
|
|
@ -50,7 +50,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
|
|||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_RMAX_DN", 5, AP_PitchController, _max_rate_neg, 0.0f),
|
||||
AP_GROUPINFO("2SRV_RMAX_DN", 5, AP_PitchController, gains.rmax_neg, 0.0f),
|
||||
|
||||
// @Param: 2SRV_RLL
|
||||
// @DisplayName: Roll compensation
|
||||
|
|
@ -60,33 +60,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("2SRV_RLL", 6, AP_PitchController, _roll_ff, 1.0f),
|
||||
|
||||
// @Param: 2SRV_IMAX
|
||||
// @DisplayName: Integrator limit
|
||||
// @Description: Limit of pitch integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
|
||||
// @Range: 0 4500
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_IMAX", 7, AP_PitchController, gains.imax, 3000),
|
||||
|
||||
// index 8 reserved for old FF
|
||||
|
||||
// @Param: 2SRV_SRMAX
|
||||
// @DisplayName: Servo slew rate limit
|
||||
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (pitch rate feedback). If the amplitude of the control action produced by the pitch rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The limit should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
|
||||
// @Units: deg/s
|
||||
// @Range: 0 500
|
||||
// @Increment: 10.0
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_SRMAX", 9, AP_PitchController, _slew_rate_max, 150.0f),
|
||||
|
||||
// @Param: 2SRV_SRTAU
|
||||
// @DisplayName: Servo slew rate decay time constant
|
||||
// @Description: This sets the time constant used to recover the D gain after it has been reduced due to excessive servo slew rate.
|
||||
// @Units: s
|
||||
// @Range: 0.5 5.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_SRTAU", 10, AP_PitchController, _slew_rate_tau, 1.0f),
|
||||
// index 7, 8 reserved for old IMAX, FF
|
||||
|
||||
// @Param: _RATE_P
|
||||
// @DisplayName: Pitch axis rate controller P gain
|
||||
|
|
@ -154,19 +128,19 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
|
|||
// @Increment: 0.5
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _RATE_STAU
|
||||
// @DisplayName: Pitch slew rate decay time constant
|
||||
// @Description: This sets the time constant used to recover the P+D gain after it has been reduced due to excessive slew rate.
|
||||
// @Units: s
|
||||
// @Range: 0.5 5.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
|
||||
AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_PitchController::AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
|
||||
: aparm(parms)
|
||||
, _ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
rate_pid.set_slew_limit_scale(45);
|
||||
}
|
||||
|
||||
/*
|
||||
AC_PID based rate controller
|
||||
*/
|
||||
|
|
@ -174,7 +148,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
{
|
||||
const float dt = AP::scheduler().get_loop_period_s();
|
||||
const float eas2tas = _ahrs.get_EAS2TAS();
|
||||
bool limit_I = fabsf(last_ac_out) >= 45;
|
||||
bool limit_I = fabsf(_last_out) >= 45;
|
||||
float rate_y = _ahrs.get_gyro().y;
|
||||
float old_I = rate_pid.get_i();
|
||||
|
||||
|
|
@ -244,8 +218,13 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
}
|
||||
|
||||
// remember the last output to trigger the I limit
|
||||
last_ac_out = out;
|
||||
_last_out = out;
|
||||
|
||||
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
autotune->update(pinfo, scaler);
|
||||
}
|
||||
|
||||
// output is scaled to notional centidegrees of deflection
|
||||
return constrain_int32(out * 100, -4500, 4500);
|
||||
}
|
||||
|
|
@ -341,10 +320,10 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
|
|||
// as the rates will be tuned when upright, and it is common that
|
||||
// much higher rates are needed inverted
|
||||
if (!inverted) {
|
||||
if (_max_rate_neg && desired_rate < -_max_rate_neg) {
|
||||
desired_rate = -_max_rate_neg;
|
||||
} else if (gains.rmax && desired_rate > gains.rmax) {
|
||||
desired_rate = gains.rmax;
|
||||
if (gains.rmax_neg && desired_rate < -gains.rmax_neg) {
|
||||
desired_rate = -gains.rmax_neg;
|
||||
} else if (gains.rmax_pos && desired_rate > gains.rmax_pos) {
|
||||
desired_rate = gains.rmax_pos;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -376,10 +355,12 @@ void AP_PitchController::convert_pid()
|
|||
}
|
||||
|
||||
float old_ff=0, old_p=1.0, old_i=0.3, old_d=0.08;
|
||||
int16_t old_imax = 3000;
|
||||
bool have_old = AP_Param::get_param_by_index(this, 1, AP_PARAM_FLOAT, &old_p);
|
||||
have_old |= AP_Param::get_param_by_index(this, 3, AP_PARAM_FLOAT, &old_i);
|
||||
have_old |= AP_Param::get_param_by_index(this, 2, AP_PARAM_FLOAT, &old_d);
|
||||
have_old |= AP_Param::get_param_by_index(this, 8, AP_PARAM_FLOAT, &old_ff);
|
||||
have_old |= AP_Param::get_param_by_index(this, 7, AP_PARAM_FLOAT, &old_imax);
|
||||
if (!have_old) {
|
||||
// none of the old gains were set
|
||||
return;
|
||||
|
|
@ -390,5 +371,34 @@ void AP_PitchController::convert_pid()
|
|||
rate_pid.kI().set_and_save_ifchanged(old_i * gains.tau);
|
||||
rate_pid.kP().set_and_save_ifchanged(old_d);
|
||||
rate_pid.kD().set_and_save_ifchanged(0);
|
||||
rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
|
||||
rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0);
|
||||
}
|
||||
|
||||
/*
|
||||
start an autotune
|
||||
*/
|
||||
void AP_PitchController::autotune_start(void)
|
||||
{
|
||||
if (autotune == nullptr) {
|
||||
autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_PITCH, aparm, rate_pid);
|
||||
if (autotune == nullptr) {
|
||||
if (!failed_autotune_alloc) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed pitch allocation");
|
||||
}
|
||||
failed_autotune_alloc = true;
|
||||
}
|
||||
}
|
||||
if (autotune != nullptr) {
|
||||
autotune->start();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
restore autotune gains
|
||||
*/
|
||||
void AP_PitchController::autotune_restore(void)
|
||||
{
|
||||
if (autotune != nullptr) {
|
||||
autotune->stop();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,19 +7,10 @@
|
|||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AC_PID/AC_PID.h>
|
||||
#include <Filter/SlewLimiter.h>
|
||||
|
||||
class AP_PitchController {
|
||||
public:
|
||||
AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
|
||||
: aparm(parms)
|
||||
, autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms, rate_pid)
|
||||
, _ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
_slew_rate_filter.set_cutoff_frequency(10.0f);
|
||||
_slew_rate_filter.reset(0.0f);
|
||||
}
|
||||
AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
|
||||
|
||||
/* Do not allow copies */
|
||||
AP_PitchController(const AP_PitchController &other) = delete;
|
||||
|
|
@ -39,8 +30,8 @@ public:
|
|||
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
|
||||
}
|
||||
|
||||
void autotune_start(void) { autotune.start(); }
|
||||
void autotune_restore(void) { autotune.stop(); }
|
||||
void autotune_start(void);
|
||||
void autotune_restore(void);
|
||||
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
|
||||
|
|
@ -56,12 +47,13 @@ public:
|
|||
private:
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune autotune;
|
||||
AP_AutoTune *autotune;
|
||||
bool failed_autotune_alloc;
|
||||
AP_Int16 _max_rate_neg;
|
||||
AP_Float _roll_ff;
|
||||
uint32_t _last_t;
|
||||
float _last_out;
|
||||
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 10, 10, 10, 0.02, 150, 1};
|
||||
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 10, 7, 4, 0.02, 100, 1};
|
||||
|
||||
AP_Logger::PID_Info _pid_info;
|
||||
|
||||
|
|
@ -69,13 +61,4 @@ private:
|
|||
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
|
||||
|
||||
AP_AHRS &_ahrs;
|
||||
|
||||
// D gain limit cycle control
|
||||
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback
|
||||
float _slew_rate_amplitude; // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
|
||||
|
||||
AP_Float _slew_rate_max; // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
|
||||
AP_Float _slew_rate_tau; // Time constant used to recover gain after a slew rate exceedance (sec)
|
||||
|
||||
float last_ac_out;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -41,35 +41,9 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
|
|||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_RMAX", 4, AP_RollController, gains.rmax, 0),
|
||||
AP_GROUPINFO("2SRV_RMAX", 4, AP_RollController, gains.rmax_pos, 0),
|
||||
|
||||
// @Param: 2SRV_IMAX
|
||||
// @DisplayName: Integrator limit
|
||||
// @Description: Limit of roll integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
|
||||
// @Range: 0 4500
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_IMAX", 5, AP_RollController, gains.imax, 3000),
|
||||
|
||||
// index 6 reserved for old FF
|
||||
|
||||
// @Param: 2SRV_SRMAX
|
||||
// @DisplayName: Servo slew rate limit
|
||||
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (roll rate feedback). If the amplitude of the control action produced by the roll rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The parameter should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A valule of zero will disable this feature.
|
||||
// @Units: deg/s
|
||||
// @Range: 0 500
|
||||
// @Increment: 10.0
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_SRMAX", 7, AP_RollController, _slew_rate_max, 150.0f),
|
||||
|
||||
// @Param: 2SRV_SRTAU
|
||||
// @DisplayName: Servo slew rate decay time constant
|
||||
// @Description: This sets the time constant used to recover the D-gain after it has been reduced due to excessive servo slew rate.
|
||||
// @Units: s
|
||||
// @Range: 0.5 5.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2SRV_SRTAU", 8, AP_RollController, _slew_rate_tau, 1.0f),
|
||||
// index 5, 6 reserved for old IMAX, FF
|
||||
|
||||
// @Param: _RATE_P
|
||||
// @DisplayName: Roll axis rate controller P gain
|
||||
|
|
@ -137,19 +111,20 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
|
|||
// @Increment: 0.5
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _RATE_STAU
|
||||
// @DisplayName: Roll slew rate decay time constant
|
||||
// @Description: This sets the time constant used to recover the P+D gain after it has been reduced due to excessive slew rate.
|
||||
// @Units: s
|
||||
// @Range: 0.5 5.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
|
||||
AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
// constructor
|
||||
AP_RollController::AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
|
||||
: aparm(parms)
|
||||
, _ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
rate_pid.set_slew_limit_scale(45);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
AC_PID based rate controller
|
||||
|
|
@ -158,7 +133,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
{
|
||||
const float dt = AP::scheduler().get_loop_period_s();
|
||||
const float eas2tas = _ahrs.get_EAS2TAS();
|
||||
bool limit_I = fabsf(last_ac_out) >= 45;
|
||||
bool limit_I = fabsf(_last_out) >= 45;
|
||||
float rate_x = _ahrs.get_gyro().x;
|
||||
float aspeed;
|
||||
float old_I = rate_pid.get_i();
|
||||
|
|
@ -212,8 +187,13 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D;
|
||||
|
||||
// remember the last output to trigger the I limit
|
||||
last_ac_out = out;
|
||||
_last_out = out;
|
||||
|
||||
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
autotune->update(pinfo, scaler);
|
||||
}
|
||||
|
||||
// output is scaled to notional centidegrees of deflection
|
||||
return constrain_int32(out * 100, -4500, 4500);
|
||||
}
|
||||
|
|
@ -249,10 +229,10 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool d
|
|||
float desired_rate = angle_err * 0.01f / gains.tau;
|
||||
|
||||
// Limit the demanded roll rate
|
||||
if (gains.rmax && desired_rate < -gains.rmax) {
|
||||
desired_rate = - gains.rmax;
|
||||
} else if (gains.rmax && desired_rate > gains.rmax) {
|
||||
desired_rate = gains.rmax;
|
||||
if (gains.rmax_pos && desired_rate < -gains.rmax_pos) {
|
||||
desired_rate = - gains.rmax_pos;
|
||||
} else if (gains.rmax_pos && desired_rate > gains.rmax_pos) {
|
||||
desired_rate = gains.rmax_pos;
|
||||
}
|
||||
|
||||
return _get_rate_out(desired_rate, scaler, disable_integrator);
|
||||
|
|
@ -275,10 +255,12 @@ void AP_RollController::convert_pid()
|
|||
return;
|
||||
}
|
||||
float old_ff=0, old_p=1.0, old_i=0.3, old_d=0.08;
|
||||
int16_t old_imax=3000;
|
||||
bool have_old = AP_Param::get_param_by_index(this, 1, AP_PARAM_FLOAT, &old_p);
|
||||
have_old |= AP_Param::get_param_by_index(this, 3, AP_PARAM_FLOAT, &old_i);
|
||||
have_old |= AP_Param::get_param_by_index(this, 2, AP_PARAM_FLOAT, &old_d);
|
||||
have_old |= AP_Param::get_param_by_index(this, 6, AP_PARAM_FLOAT, &old_ff);
|
||||
have_old |= AP_Param::get_param_by_index(this, 5, AP_PARAM_INT16, &old_imax);
|
||||
if (!have_old) {
|
||||
// none of the old gains were set
|
||||
return;
|
||||
|
|
@ -289,5 +271,34 @@ void AP_RollController::convert_pid()
|
|||
rate_pid.kI().set_and_save_ifchanged(old_i * gains.tau);
|
||||
rate_pid.kP().set_and_save_ifchanged(old_d);
|
||||
rate_pid.kD().set_and_save_ifchanged(0);
|
||||
rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
|
||||
rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0);
|
||||
}
|
||||
|
||||
/*
|
||||
start an autotune
|
||||
*/
|
||||
void AP_RollController::autotune_start(void)
|
||||
{
|
||||
if (autotune == nullptr) {
|
||||
autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_ROLL, aparm, rate_pid);
|
||||
if (autotune == nullptr) {
|
||||
if (!failed_autotune_alloc) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed roll allocation");
|
||||
}
|
||||
failed_autotune_alloc = true;
|
||||
}
|
||||
}
|
||||
if (autotune != nullptr) {
|
||||
autotune->start();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
restore autotune gains
|
||||
*/
|
||||
void AP_RollController::autotune_restore(void)
|
||||
{
|
||||
if (autotune != nullptr) {
|
||||
autotune->stop();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -10,15 +10,7 @@
|
|||
|
||||
class AP_RollController {
|
||||
public:
|
||||
AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
|
||||
: aparm(parms)
|
||||
, autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms, rate_pid)
|
||||
, _ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
_slew_rate_filter.set_cutoff_frequency(10.0f);
|
||||
_slew_rate_filter.reset(0.0f);
|
||||
}
|
||||
AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
|
||||
|
||||
/* Do not allow copies */
|
||||
AP_RollController(const AP_RollController &other) = delete;
|
||||
|
|
@ -38,8 +30,8 @@ public:
|
|||
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
|
||||
}
|
||||
|
||||
void autotune_start(void) { autotune.start(); }
|
||||
void autotune_restore(void) { autotune.stop(); }
|
||||
void autotune_start(void);
|
||||
void autotune_restore(void);
|
||||
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
|
||||
|
|
@ -62,21 +54,15 @@ public:
|
|||
private:
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune autotune;
|
||||
AP_AutoTune *autotune;
|
||||
bool failed_autotune_alloc;
|
||||
uint32_t _last_t;
|
||||
float _last_out;
|
||||
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 10, 10, 10, 0.02, 150, 1};
|
||||
float last_ac_out;
|
||||
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 10, 7, 4, 0.02, 100, 1};
|
||||
|
||||
AP_Logger::PID_Info _pid_info;
|
||||
|
||||
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
|
||||
|
||||
AP_AHRS &_ahrs;
|
||||
|
||||
// D gain limit cycle control
|
||||
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback
|
||||
float _slew_rate_amplitude; // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
|
||||
AP_Float _slew_rate_max; // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
|
||||
AP_Float _slew_rate_tau; // Time constant used to recover gain after a slew rate exceedance (sec)
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue