mirror of https://github.com/ArduPilot/ardupilot
APM_Control: first version of APM_Control autotuning
this adds autotune to the roll/pitch controllers using a very simple mechanism. The plan is that this provides a framework which Paul and Jon will build upon.
This commit is contained in:
Andrew Tridgell
parent
3b8839d2f7
commit
50fc75917e
|
|
@ -0,0 +1,243 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
The strategy for roll/pitch autotune is to give the user a AUTOTUNE
|
||||
flight mode which behaves just like FBWA, but does automatic
|
||||
tuning.
|
||||
|
||||
While the user is flying in AUTOTUNE the gains are saved every 10
|
||||
seconds, but the saved gains are not the current gains, instead it
|
||||
saves the gains from 10s ago. When the user exits AUTOTUNE the
|
||||
gains are restored from 10s ago.
|
||||
|
||||
This allows the user to fly as much as they want in AUTOTUNE mode,
|
||||
and if they are ever unhappy they just exit the mode. If they stay
|
||||
in AUTOTUNE for more than 10s then their gains will have changed.
|
||||
|
||||
Using this approach users don't need any special switches, they
|
||||
just need to be able to enter and exit AUTOTUNE mode
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include "AP_AutoTune.h"
|
||||
|
||||
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
|
||||
#define AUTOTUNE_INCREASE_STEP 0.05f
|
||||
|
||||
// step size for decreasing gains
|
||||
#define AUTOTUNE_DECREASE_STEP 0.1f
|
||||
|
||||
// max rate
|
||||
#define AUTOTUNE_MAX_RATE 60
|
||||
#define AUTOTUNE_MIN_RATE 30
|
||||
|
||||
// min/max P gains
|
||||
#define AUTOTUNE_MAX_P 4.0f
|
||||
#define AUTOTUNE_MIN_P 0.3f
|
||||
|
||||
// constructor
|
||||
AP_AutoTune::AP_AutoTune(ATGains &_gains) :
|
||||
current(_gains) {}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
#include <stdio.h>
|
||||
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
#else
|
||||
# define Debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
/*
|
||||
start an autotune session
|
||||
*/
|
||||
void AP_AutoTune::start(void)
|
||||
{
|
||||
running = true;
|
||||
state = DEMAND_UNSATURATED;
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
|
||||
state_enter_ms = now;
|
||||
last_save_ms = now;
|
||||
|
||||
restore = current;
|
||||
|
||||
/*
|
||||
set some reasonable values for I and D if the user hasn't set
|
||||
them at all
|
||||
*/
|
||||
if (current.I < 0.1f) {
|
||||
current.I.set(0.1f);
|
||||
}
|
||||
if (current.I > 0.5f) {
|
||||
current.I.set(0.5f);
|
||||
}
|
||||
if (current.imax < 2000) {
|
||||
current.imax.set(2000);
|
||||
}
|
||||
if (current.rmax <= 0 || current.rmax > AUTOTUNE_MAX_RATE) {
|
||||
current.rmax.set(AUTOTUNE_MAX_RATE);
|
||||
}
|
||||
if (current.rmax < AUTOTUNE_MIN_RATE) {
|
||||
current.rmax.set(AUTOTUNE_MIN_RATE);
|
||||
}
|
||||
if (current.D < 0.05f) {
|
||||
current.D.set(0.05f);
|
||||
}
|
||||
if (current.D > 0.5f) {
|
||||
current.D.set(0.5f);
|
||||
}
|
||||
|
||||
next_save = current;
|
||||
|
||||
Debug("START P -> %.3f\n", current.P.get());
|
||||
}
|
||||
|
||||
/*
|
||||
called when we change state to see if we should change gains
|
||||
*/
|
||||
void AP_AutoTune::stop(void)
|
||||
{
|
||||
running = false;
|
||||
save_gains(restore);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
one update cycle of the autotuner
|
||||
*/
|
||||
void AP_AutoTune::update(float desired_rate, float achieved_rate, float servo_out)
|
||||
{
|
||||
if (!running) {
|
||||
return;
|
||||
}
|
||||
check_save();
|
||||
|
||||
// see what state we are in
|
||||
ATState new_state;
|
||||
float abs_desired_rate = fabsf(desired_rate);
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
|
||||
if (fabsf(servo_out) >= 4500) {
|
||||
// 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;
|
||||
}
|
||||
|
||||
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;
|
||||
} else {
|
||||
new_state = desired_rate > 0 ? DEMAND_UNDER_POS : DEMAND_UNDER_NEG;
|
||||
}
|
||||
if (new_state != state) {
|
||||
check_state_exit(now - state_enter_ms);
|
||||
state = new_state;
|
||||
state_enter_ms = now;
|
||||
saturated_surfaces = false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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) {
|
||||
current.P += AUTOTUNE_INCREASE_STEP;
|
||||
if (current.P > AUTOTUNE_MAX_P) {
|
||||
current.P = AUTOTUNE_MAX_P;
|
||||
}
|
||||
Debug("UNDER P -> %.3f\n", current.P.get());
|
||||
}
|
||||
break;
|
||||
case DEMAND_OVER_POS:
|
||||
case DEMAND_OVER_NEG:
|
||||
if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) {
|
||||
current.P -= AUTOTUNE_DECREASE_STEP;
|
||||
if (current.P < AUTOTUNE_MIN_P) {
|
||||
current.P = AUTOTUNE_MIN_P;
|
||||
}
|
||||
Debug("OVER P -> %.3f\n", current.P.get());
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
see if we should save new values
|
||||
*/
|
||||
void AP_AutoTune::check_save(void)
|
||||
{
|
||||
if (hal.scheduler->millis() - last_save_ms < AUTOTUNE_SAVE_PERIOD) {
|
||||
return;
|
||||
}
|
||||
|
||||
// save the next_save values, which are the autotune value from
|
||||
// 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;
|
||||
|
||||
save_gains(next_save);
|
||||
Debug("SAVE P -> %.3f\n", current.P.get());
|
||||
|
||||
// restore our current gains
|
||||
current = 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;
|
||||
last_save_ms = hal.scheduler->millis();
|
||||
}
|
||||
|
||||
/*
|
||||
save a set of gains
|
||||
*/
|
||||
void AP_AutoTune::save_gains(const ATGains &v)
|
||||
{
|
||||
current.tau.set_and_save_ifchanged(v.tau);
|
||||
current.P.set_and_save_ifchanged(v.P);
|
||||
current.I.set_and_save_ifchanged(v.I);
|
||||
current.D.set_and_save_ifchanged(v.D);
|
||||
current.rmax.set_and_save_ifchanged(v.rmax);
|
||||
current.imax.set_and_save_ifchanged(v.imax);
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef __AP_AUTOTUNE_H__
|
||||
#define __AP_AUTOTUNE_H__
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Param.h>
|
||||
|
||||
class AP_AutoTune {
|
||||
public:
|
||||
struct ATGains {
|
||||
AP_Float tau;
|
||||
AP_Float P;
|
||||
AP_Float I;
|
||||
AP_Float D;
|
||||
AP_Int16 rmax;
|
||||
AP_Int16 imax;
|
||||
};
|
||||
|
||||
// constructor
|
||||
AP_AutoTune(ATGains &_gains);
|
||||
|
||||
// called when autotune mode is entered
|
||||
void start(void);
|
||||
|
||||
// called to stop autotune and restore gains when user leaves
|
||||
// autotune
|
||||
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);
|
||||
|
||||
// are we running?
|
||||
bool running:1;
|
||||
|
||||
private:
|
||||
// the current gains
|
||||
ATGains ¤t;
|
||||
|
||||
// did we saturate surfaces?
|
||||
bool saturated_surfaces:1;
|
||||
|
||||
// values to restore if we leave autotune mode
|
||||
ATGains restore;
|
||||
|
||||
// values to save on the next save event
|
||||
ATGains next_save;
|
||||
|
||||
// time when we last saved
|
||||
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;
|
||||
|
||||
// when we entered the current state
|
||||
uint32_t state_enter_ms;
|
||||
|
||||
void check_save(void);
|
||||
void check_state_exit(uint32_t state_time_ms);
|
||||
void save_gains(const ATGains &v);
|
||||
};
|
||||
|
||||
#endif // __AP_AUTOTUNE_H__
|
||||
|
||||
|
|
@ -33,7 +33,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
|
|||
// @Units: seconds
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TCONST", 0, AP_PitchController, _tau, 0.5f),
|
||||
AP_GROUPINFO("TCONST", 0, AP_PitchController, gains.tau, 0.5f),
|
||||
|
||||
// @Param: P
|
||||
// @DisplayName: Proportional Gain
|
||||
|
|
@ -41,7 +41,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
|
|||
// @Range: 0.1 2.0
|
||||
// @Increment: 0.1
|
||||
// @User: User
|
||||
AP_GROUPINFO("P", 1, AP_PitchController, _K_P, 0.4f),
|
||||
AP_GROUPINFO("P", 1, AP_PitchController, gains.P, 0.4f),
|
||||
|
||||
// @Param: D
|
||||
// @DisplayName: Damping Gain
|
||||
|
|
@ -49,7 +49,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
|
|||
// @Range: 0 0.1
|
||||
// @Increment: 0.01
|
||||
// @User: User
|
||||
AP_GROUPINFO("D", 2, AP_PitchController, _K_D, 0.02f),
|
||||
AP_GROUPINFO("D", 2, AP_PitchController, gains.D, 0.02f),
|
||||
|
||||
// @Param: I
|
||||
// @DisplayName: Integrator Gain
|
||||
|
|
@ -57,7 +57,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
|
|||
// @Range: 0 0.5
|
||||
// @Increment: 0.05
|
||||
// @User: User
|
||||
AP_GROUPINFO("I", 3, AP_PitchController, _K_I, 0.0f),
|
||||
AP_GROUPINFO("I", 3, AP_PitchController, gains.I, 0.0f),
|
||||
|
||||
// @Param: RMAX_UP
|
||||
// @DisplayName: Pitch up max rate
|
||||
|
|
@ -66,7 +66,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
|
|||
// @Units: degrees/second
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RMAX_UP", 4, AP_PitchController, _max_rate_pos, 0.0f),
|
||||
AP_GROUPINFO("RMAX_UP", 4, AP_PitchController, gains.rmax, 0.0f),
|
||||
|
||||
// @Param: RMAX_DN
|
||||
// @DisplayName: Pitch down max rate
|
||||
|
|
@ -91,7 +91,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
|
|||
// @Range: 0 4500
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("IMAX", 7, AP_PitchController, _imax, 1500),
|
||||
AP_GROUPINFO("IMAX", 7, AP_PitchController, gains.imax, 1500),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
@ -122,14 +122,15 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
float omega_y = _ahrs.get_gyro().y;
|
||||
|
||||
// Calculate the pitch rate error (deg/sec) and scale
|
||||
float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
|
||||
float achieved_rate = ToDeg(omega_y);
|
||||
float rate_error = (desired_rate - achieved_rate) * scaler;
|
||||
|
||||
// Multiply pitch rate error by _ki_rate and integrate
|
||||
// Scaler is applied before integrator so that integrator state relates directly to elevator deflection
|
||||
// This means elevator trim offset doesn't change as the value of scaler changes with airspeed
|
||||
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
|
||||
if (!disable_integrator && _K_I > 0) {
|
||||
float ki_rate = _K_I * _tau;
|
||||
if (!disable_integrator && gains.I > 0) {
|
||||
float ki_rate = gains.I * gains.tau;
|
||||
//only integrate if gain and time step are positive and airspeed above min value.
|
||||
if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
|
||||
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
|
||||
|
|
@ -147,20 +148,35 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
}
|
||||
|
||||
// Scale the integration limit
|
||||
float intLimScaled = _imax * 0.01f;
|
||||
float intLimScaled = gains.imax * 0.01f;
|
||||
|
||||
// Constrain the integrator state
|
||||
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
|
||||
|
||||
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
|
||||
// No conversion is required for K_D
|
||||
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) / _ahrs.get_EAS2TAS();
|
||||
float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / _ahrs.get_EAS2TAS();
|
||||
|
||||
// Calculate the demanded control surface deflection
|
||||
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
|
||||
// path, but want a 1/speed^2 scaler applied to the rate error path.
|
||||
// This is because acceleration scales with speed^2, but rate scales with speed.
|
||||
_last_out = ( (rate_error * _K_D) + (desired_rate * kp_ff) ) * scaler + _integrator;
|
||||
_last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler;
|
||||
|
||||
if (autotune.running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
// Note that we don't pass the integrator component so we get
|
||||
// a better idea of how much the base PD controller
|
||||
// contributed
|
||||
autotune.update(desired_rate, achieved_rate, _last_out);
|
||||
|
||||
// limit pitch down rate to rate up
|
||||
if (_max_rate_neg == 0 || _max_rate_neg > gains.rmax) {
|
||||
_max_rate_neg = gains.rmax;
|
||||
}
|
||||
}
|
||||
|
||||
_last_out += _integrator;
|
||||
|
||||
// Convert to centi-degrees and constrain
|
||||
return constrain_float(_last_out * 100, -4500, 4500);
|
||||
|
|
@ -244,14 +260,14 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
|
|||
float rate_offset;
|
||||
bool inverted;
|
||||
|
||||
if (_tau < 0.1) {
|
||||
_tau = 0.1;
|
||||
if (gains.tau < 0.1f) {
|
||||
gains.tau.set(0.1f);
|
||||
}
|
||||
|
||||
rate_offset = _get_coordination_rate_offset(aspeed, inverted);
|
||||
|
||||
// Calculate the desired pitch rate (deg/sec) from the angle error
|
||||
float desired_rate = angle_err * 0.01f / _tau;
|
||||
float desired_rate = angle_err * 0.01f / gains.tau;
|
||||
|
||||
// limit the maximum pitch rate demand. Don't apply when inverted
|
||||
// as the rates will be tuned when upright, and it is common that
|
||||
|
|
@ -259,8 +275,8 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
|
|||
if (!inverted) {
|
||||
if (_max_rate_neg && desired_rate < -_max_rate_neg) {
|
||||
desired_rate = -_max_rate_neg;
|
||||
} else if (_max_rate_pos && desired_rate > _max_rate_pos) {
|
||||
desired_rate = _max_rate_pos;
|
||||
} else if (gains.rmax && desired_rate > gains.rmax) {
|
||||
desired_rate = gains.rmax;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -6,12 +6,14 @@
|
|||
#include <AP_AHRS.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Vehicle.h>
|
||||
#include <AP_AutoTune.h>
|
||||
#include <math.h>
|
||||
|
||||
class AP_PitchController {
|
||||
public:
|
||||
AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
|
||||
aparm(parms),
|
||||
autotune(gains),
|
||||
_ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
|
@ -22,18 +24,17 @@ public:
|
|||
|
||||
void reset_I();
|
||||
|
||||
void autotune_start(void) { autotune.start(); }
|
||||
void autotune_restore(void) { autotune.stop(); }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
private:
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
AP_Float _tau;
|
||||
AP_Float _K_P;
|
||||
AP_Float _K_I;
|
||||
AP_Float _K_D;
|
||||
AP_Int16 _max_rate_pos;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune autotune;
|
||||
AP_Int16 _max_rate_neg;
|
||||
AP_Float _roll_ff;
|
||||
AP_Int16 _imax;
|
||||
uint32_t _last_t;
|
||||
float _last_out;
|
||||
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
|
|||
// @Units: seconds
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TCONST", 0, AP_RollController, _tau, 0.5f),
|
||||
AP_GROUPINFO("TCONST", 0, AP_RollController, gains.tau, 0.5f),
|
||||
|
||||
// @Param: P
|
||||
// @DisplayName: Proportional Gain
|
||||
|
|
@ -40,7 +40,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
|
|||
// @Range: 0.1 2.0
|
||||
// @Increment: 0.1
|
||||
// @User: User
|
||||
AP_GROUPINFO("P", 1, AP_RollController, _K_P, 0.4f),
|
||||
AP_GROUPINFO("P", 1, AP_RollController, gains.P, 0.4f),
|
||||
|
||||
// @Param: D
|
||||
// @DisplayName: Damping Gain
|
||||
|
|
@ -48,7 +48,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
|
|||
// @Range: 0 0.1
|
||||
// @Increment: 0.01
|
||||
// @User: User
|
||||
AP_GROUPINFO("D", 2, AP_RollController, _K_D, 0.02f),
|
||||
AP_GROUPINFO("D", 2, AP_RollController, gains.D, 0.02f),
|
||||
|
||||
// @Param: I
|
||||
// @DisplayName: Integrator Gain
|
||||
|
|
@ -56,7 +56,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
|
|||
// @Range: 0 1.0
|
||||
// @Increment: 0.05
|
||||
// @User: User
|
||||
AP_GROUPINFO("I", 3, AP_RollController, _K_I, 0.0f),
|
||||
AP_GROUPINFO("I", 3, AP_RollController, gains.I, 0.0f),
|
||||
|
||||
// @Param: RMAX
|
||||
// @DisplayName: Maximum Roll Rate
|
||||
|
|
@ -65,7 +65,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
|
|||
// @Units: degrees/second
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RMAX", 4, AP_RollController, _max_rate, 0),
|
||||
AP_GROUPINFO("RMAX", 4, AP_RollController, gains.rmax, 0),
|
||||
|
||||
// @Param: IMAX
|
||||
// @DisplayName: Integrator limit
|
||||
|
|
@ -73,7 +73,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
|
|||
// @Range: 0 4500
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("IMAX", 5, AP_RollController, _imax, 1500),
|
||||
AP_GROUPINFO("IMAX", 5, AP_RollController, gains.imax, 1500),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
@ -94,22 +94,23 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
|
||||
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
|
||||
// No conversion is required for K_D
|
||||
float ki_rate = _K_I * _tau;
|
||||
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0)/_ahrs.get_EAS2TAS();
|
||||
float ki_rate = gains.I * gains.tau;
|
||||
float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0)/_ahrs.get_EAS2TAS();
|
||||
float delta_time = (float)dt * 0.001f;
|
||||
|
||||
// Limit the demanded roll rate
|
||||
if (_max_rate && desired_rate < -_max_rate) {
|
||||
desired_rate = -_max_rate;
|
||||
} else if (_max_rate && desired_rate > _max_rate) {
|
||||
desired_rate = _max_rate;
|
||||
if (gains.rmax && desired_rate < -gains.rmax) {
|
||||
desired_rate = - gains.rmax;
|
||||
} else if (gains.rmax && desired_rate > gains.rmax) {
|
||||
desired_rate = gains.rmax;
|
||||
}
|
||||
|
||||
// Get body rate vector (radians/sec)
|
||||
float omega_x = _ahrs.get_gyro().x;
|
||||
|
||||
// Calculate the roll rate error (deg/sec) and apply gain scaler
|
||||
float rate_error = (desired_rate - ToDeg(omega_x)) * scaler;
|
||||
float achieved_rate = ToDeg(omega_x);
|
||||
float rate_error = (desired_rate - achieved_rate) * scaler;
|
||||
|
||||
// Get an airspeed estimate - default to zero if none available
|
||||
float aspeed;
|
||||
|
|
@ -139,7 +140,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
}
|
||||
|
||||
// Scale the integration limit
|
||||
float intLimScaled = _imax * 0.01f;
|
||||
float intLimScaled = gains.imax * 0.01f;
|
||||
|
||||
// Constrain the integrator state
|
||||
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
|
||||
|
|
@ -148,7 +149,17 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
|
|||
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
|
||||
// path, but want a 1/speed^2 scaler applied to the rate error path.
|
||||
// This is because acceleration scales with speed^2, but rate scales with speed.
|
||||
_last_out = ( (rate_error * _K_D) + (desired_rate * kp_ff) ) * scaler + _integrator;
|
||||
_last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler;
|
||||
|
||||
if (autotune.running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
// Note that we don't pass the integrator component so we get
|
||||
// a better idea of how much the base PD controller
|
||||
// contributed
|
||||
autotune.update(desired_rate, achieved_rate, _last_out);
|
||||
}
|
||||
|
||||
_last_out += _integrator;
|
||||
|
||||
// Convert to centi-degrees and constrain
|
||||
return constrain_float(_last_out * 100, -4500, 4500);
|
||||
|
|
@ -178,12 +189,12 @@ int32_t AP_RollController::get_rate_out(float desired_rate, float scaler)
|
|||
*/
|
||||
int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator)
|
||||
{
|
||||
if (_tau < 0.1) {
|
||||
_tau = 0.1;
|
||||
if (gains.tau < 0.1) {
|
||||
gains.tau.set(0.1f);
|
||||
}
|
||||
|
||||
// Calculate the desired roll rate (deg/sec) from the angle error
|
||||
float desired_rate = angle_err * 0.01f / _tau;
|
||||
float desired_rate = angle_err * 0.01f / gains.tau;
|
||||
|
||||
return _get_rate_out(desired_rate, scaler, disable_integrator);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,12 +6,14 @@
|
|||
#include <AP_AHRS.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Vehicle.h>
|
||||
#include <AP_AutoTune.h>
|
||||
#include <math.h>
|
||||
|
||||
class AP_RollController {
|
||||
public:
|
||||
AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
|
||||
aparm(parms),
|
||||
autotune(gains),
|
||||
_ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
|
@ -22,16 +24,15 @@ public:
|
|||
|
||||
void reset_I();
|
||||
|
||||
void autotune_start(void) { autotune.start(); }
|
||||
void autotune_restore(void) { autotune.stop(); }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
private:
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
AP_Float _tau;
|
||||
AP_Float _K_P;
|
||||
AP_Float _K_I;
|
||||
AP_Float _K_D;
|
||||
AP_Int16 _max_rate;
|
||||
AP_Int16 _imax;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune autotune;
|
||||
uint32_t _last_t;
|
||||
float _last_out;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue