2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
#if AUTOTUNE_ENABLED == ENABLED
|
|
|
|
|
|
|
|
/*
|
2016-07-25 15:45:29 -03:00
|
|
|
* Init and run calls for autotune flight mode
|
2014-02-02 03:57:34 -04:00
|
|
|
*
|
|
|
|
* Instructions:
|
|
|
|
* 1) Set up one flight mode switch position to be AltHold.
|
|
|
|
* 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch.
|
|
|
|
* 3) Ensure the ch7 or ch8 switch is in the LOW position.
|
|
|
|
* 4) Wait for a calm day and go to a large open area.
|
|
|
|
* 5) Take off and put the vehicle into AltHold mode at a comfortable altitude.
|
|
|
|
* 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning:
|
|
|
|
* a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back.
|
|
|
|
* b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests).
|
|
|
|
* When you release the sticks it will continue auto tuning where it left off.
|
|
|
|
* c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs.
|
|
|
|
* d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered.
|
|
|
|
* 7) When the tune completes the vehicle will change back to the original PID gains.
|
|
|
|
* 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains.
|
|
|
|
* 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains.
|
|
|
|
* 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently.
|
|
|
|
* If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm
|
|
|
|
*
|
|
|
|
* What it's doing during each "twitch":
|
|
|
|
* a) invokes 90 deg/sec rate request
|
|
|
|
* b) records maximum "forward" roll rate and bounce back rate
|
|
|
|
* c) when copter reaches 20 degrees or 1 second has passed, it commands level
|
|
|
|
* d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P
|
|
|
|
* e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec)
|
|
|
|
* f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec)
|
2014-12-05 08:38:28 -04:00
|
|
|
* g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec)
|
2014-02-02 03:57:34 -04:00
|
|
|
* h) invokes a 20deg angle request on roll or pitch
|
|
|
|
* i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg)
|
|
|
|
* j) decreases stab P by 25%
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2014-12-05 08:38:28 -04:00
|
|
|
#define AUTOTUNE_AXIS_BITMASK_ROLL 1
|
|
|
|
#define AUTOTUNE_AXIS_BITMASK_PITCH 2
|
|
|
|
#define AUTOTUNE_AXIS_BITMASK_YAW 4
|
|
|
|
|
2015-03-06 04:19:31 -04:00
|
|
|
#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
|
2016-10-23 04:05:06 -03:00
|
|
|
#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 1000 // timeout for tuning mode's testing step
|
2017-05-22 20:24:14 -03:00
|
|
|
#define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level
|
2015-04-14 09:58:57 -03:00
|
|
|
#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch
|
|
|
|
#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
|
|
|
|
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level
|
2015-03-06 04:19:31 -04:00
|
|
|
#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term
|
|
|
|
#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term
|
|
|
|
#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
|
|
|
|
#define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
|
2015-05-04 08:37:06 -03:00
|
|
|
#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing
|
2015-03-06 04:19:31 -04:00
|
|
|
#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing
|
2016-02-18 07:43:08 -04:00
|
|
|
#define AUTOTUNE_RD_MAX 0.200f // maximum Rate D value
|
2015-03-21 10:56:30 -03:00
|
|
|
#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value
|
2015-06-18 09:20:14 -03:00
|
|
|
#define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value
|
2015-03-06 04:19:31 -04:00
|
|
|
#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value
|
2015-04-16 07:08:31 -03:00
|
|
|
#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value
|
2015-03-06 04:19:31 -04:00
|
|
|
#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value
|
2015-04-14 09:58:57 -03:00
|
|
|
#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value
|
2016-02-18 07:43:08 -04:00
|
|
|
#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch
|
|
|
|
#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw
|
2015-06-18 09:20:14 -03:00
|
|
|
#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw
|
|
|
|
#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains
|
2014-12-05 08:38:28 -04:00
|
|
|
#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in
|
2015-05-04 08:37:06 -03:00
|
|
|
#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
|
|
|
|
#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
|
2015-05-16 23:05:48 -03:00
|
|
|
#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
|
2015-05-04 08:37:06 -03:00
|
|
|
#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
|
2015-06-18 09:20:14 -03:00
|
|
|
#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
|
2014-12-05 08:38:28 -04:00
|
|
|
|
|
|
|
// roll and pitch axes
|
2015-03-06 04:19:31 -04:00
|
|
|
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
|
|
|
|
#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 9000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
|
2015-06-18 09:20:14 -03:00
|
|
|
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
|
2014-12-05 08:38:28 -04:00
|
|
|
#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
|
|
|
|
|
|
|
|
// yaw axis
|
2015-06-18 09:20:14 -03:00
|
|
|
#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
|
|
|
|
#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step
|
|
|
|
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step
|
|
|
|
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// Auto Tune message ids for ground station
|
|
|
|
#define AUTOTUNE_MESSAGE_STARTED 0
|
|
|
|
#define AUTOTUNE_MESSAGE_STOPPED 1
|
|
|
|
#define AUTOTUNE_MESSAGE_SUCCESS 2
|
|
|
|
#define AUTOTUNE_MESSAGE_FAILED 3
|
2014-12-05 08:38:28 -04:00
|
|
|
#define AUTOTUNE_MESSAGE_SAVED_GAINS 4
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-02-07 20:35:31 -04:00
|
|
|
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
|
|
|
|
|
2014-10-24 16:39:54 -03:00
|
|
|
// autotune_init - should be called when autotune mode is selected
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeAutoTune::init(bool ignore_checks)
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2014-11-05 08:04:59 -04:00
|
|
|
bool success = true;
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (mode) {
|
|
|
|
case FAILED:
|
2016-12-12 08:36:38 -04:00
|
|
|
// autotune has been run but failed so reset state to uninitialized
|
2017-12-06 01:39:18 -04:00
|
|
|
mode = UNINITIALISED;
|
2017-08-22 14:28:10 -03:00
|
|
|
// fall through to restart the tuning
|
|
|
|
FALLTHROUGH;
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
case UNINITIALISED:
|
2014-02-02 03:57:34 -04:00
|
|
|
// autotune has never been run
|
2017-12-06 01:39:18 -04:00
|
|
|
success = start(false);
|
2014-11-05 08:04:59 -04:00
|
|
|
if (success) {
|
2014-02-02 03:57:34 -04:00
|
|
|
// so store current gains as original gains
|
2017-12-06 01:39:18 -04:00
|
|
|
backup_gains_and_initialise();
|
2014-02-02 03:57:34 -04:00
|
|
|
// advance mode to tuning
|
2017-12-06 01:39:18 -04:00
|
|
|
mode = TUNING;
|
2014-02-02 03:57:34 -04:00
|
|
|
// send message to ground station that we've started tuning
|
2017-12-06 01:39:18 -04:00
|
|
|
update_gcs(AUTOTUNE_MESSAGE_STARTED);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
case TUNING:
|
2014-02-02 03:57:34 -04:00
|
|
|
// we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off
|
2017-12-06 01:39:18 -04:00
|
|
|
success = start(false);
|
2014-11-05 08:04:59 -04:00
|
|
|
if (success) {
|
2014-02-02 03:57:34 -04:00
|
|
|
// reset gains to tuning-start gains (i.e. low I term)
|
2017-12-06 01:39:18 -04:00
|
|
|
load_intra_test_gains();
|
2014-02-02 03:57:34 -04:00
|
|
|
// write dataflash log even and send message to ground station
|
|
|
|
Log_Write_Event(DATA_AUTOTUNE_RESTART);
|
2017-12-06 01:39:18 -04:00
|
|
|
update_gcs(AUTOTUNE_MESSAGE_STARTED);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
case SUCCESS:
|
2014-02-02 03:57:34 -04:00
|
|
|
// we have completed a tune and the pilot wishes to test the new gains in the current flight mode
|
|
|
|
// so simply apply tuning gains (i.e. do not change flight mode)
|
2017-12-06 01:39:18 -04:00
|
|
|
load_tuned_gains();
|
2014-02-02 03:57:34 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING);
|
|
|
|
break;
|
|
|
|
}
|
2014-11-05 08:04:59 -04:00
|
|
|
|
2017-01-02 21:45:35 -04:00
|
|
|
// only do position hold if starting autotune from LOITER or POSHOLD
|
2017-12-06 01:39:18 -04:00
|
|
|
use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD);
|
|
|
|
have_position = false;
|
2017-01-02 21:45:35 -04:00
|
|
|
|
2014-10-24 16:39:54 -03:00
|
|
|
return success;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// stop - should be called when the ch7/ch8 switch is switched OFF
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::stop()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
|
|
|
// set gains to their original values
|
2017-12-06 01:39:18 -04:00
|
|
|
load_orig_gains();
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// re-enable angle-to-rate request limits
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->use_ff_and_input_shaping(true);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2014-12-05 08:38:28 -04:00
|
|
|
// log off event and send message to ground station
|
2017-12-06 01:39:18 -04:00
|
|
|
update_gcs(AUTOTUNE_MESSAGE_STOPPED);
|
2014-02-02 03:57:34 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_OFF);
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// Note: we leave the mode as it was so that we know how the autotune ended
|
2014-02-02 03:57:34 -04:00
|
|
|
// we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// start - Initialize autotune flight mode
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeAutoTune::start(bool ignore_checks)
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2017-01-02 21:45:35 -04:00
|
|
|
// only allow flip from Stabilize, AltHold, PosHold or Loiter modes
|
2016-03-22 20:36:52 -03:00
|
|
|
if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD &&
|
|
|
|
_copter.control_mode != LOITER && _copter.control_mode != POSHOLD) {
|
2014-02-02 03:57:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure throttle is above zero
|
2015-03-13 16:38:23 -03:00
|
|
|
if (ap.throttle_zero) {
|
2014-02-02 03:57:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure we are flying
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
|
2014-02-02 03:57:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-11-18 15:08:19 -04:00
|
|
|
// initialize vertical speeds and leash lengths
|
2017-11-08 09:25:53 -04:00
|
|
|
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_accel_z(g.pilot_accel_z);
|
2014-04-29 23:17:59 -03:00
|
|
|
|
2015-10-27 10:06:50 -03:00
|
|
|
// initialise position and desired velocity
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!pos_control->is_active_z()) {
|
|
|
|
pos_control->set_alt_target_to_current_alt();
|
|
|
|
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
2016-10-14 09:28:32 -03:00
|
|
|
}
|
2014-04-30 04:29:32 -03:00
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
const char *Copter::ModeAutoTune::level_issue_string() const
|
2017-02-12 21:49:55 -04:00
|
|
|
{
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (level_problem.issue) {
|
|
|
|
case LEVEL_ISSUE_NONE:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "None";
|
2017-12-06 01:39:18 -04:00
|
|
|
case LEVEL_ISSUE_ANGLE_ROLL:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Angle(R)";
|
2017-12-06 01:39:18 -04:00
|
|
|
case LEVEL_ISSUE_ANGLE_PITCH:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Angle(P)";
|
2017-12-06 01:39:18 -04:00
|
|
|
case LEVEL_ISSUE_ANGLE_YAW:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Angle(Y)";
|
2017-12-06 01:39:18 -04:00
|
|
|
case LEVEL_ISSUE_RATE_ROLL:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Rate(R)";
|
2017-12-06 01:39:18 -04:00
|
|
|
case LEVEL_ISSUE_RATE_PITCH:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Rate(P)";
|
2017-12-06 01:39:18 -04:00
|
|
|
case LEVEL_ISSUE_RATE_YAW:
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Rate(Y)";
|
|
|
|
}
|
|
|
|
return "Bug";
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::send_step_string()
|
2017-02-07 20:35:31 -04:00
|
|
|
{
|
2017-12-06 01:39:18 -04:00
|
|
|
if (pilot_override) {
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
|
2017-02-12 21:49:55 -04:00
|
|
|
return;
|
2017-02-07 20:35:31 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (step) {
|
|
|
|
case WAITING_FOR_LEVEL:
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: WFL (%s) (%f > %f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f));
|
2017-02-12 21:49:55 -04:00
|
|
|
return;
|
2017-12-06 01:39:18 -04:00
|
|
|
case UPDATE_GAINS:
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: UPDATING_GAINS");
|
2017-02-12 21:49:55 -04:00
|
|
|
return;
|
2017-12-06 01:39:18 -04:00
|
|
|
case TWITCHING:
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: TWITCHING");
|
2017-02-12 21:49:55 -04:00
|
|
|
return;
|
2017-02-07 20:35:31 -04:00
|
|
|
}
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step");
|
2017-02-07 20:35:31 -04:00
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
const char *Copter::ModeAutoTune::type_string() const
|
2017-02-07 20:35:31 -04:00
|
|
|
{
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (tune_type) {
|
|
|
|
case RD_UP:
|
2017-02-07 20:35:31 -04:00
|
|
|
return "Rate D Up";
|
2017-12-06 01:39:18 -04:00
|
|
|
case RD_DOWN:
|
2017-02-07 20:35:31 -04:00
|
|
|
return "Rate D Down";
|
2017-12-06 01:39:18 -04:00
|
|
|
case RP_UP:
|
2017-02-07 20:35:31 -04:00
|
|
|
return "Rate P Up";
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_DOWN:
|
2017-02-07 20:35:31 -04:00
|
|
|
return "Angle P Down";
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_UP:
|
2017-02-07 20:35:31 -04:00
|
|
|
return "Angle P Up";
|
|
|
|
}
|
2017-02-12 21:49:55 -04:00
|
|
|
return "Bug";
|
2017-02-07 20:35:31 -04:00
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::do_gcs_announcements()
|
2017-02-07 20:35:31 -04:00
|
|
|
{
|
|
|
|
const uint32_t now = millis();
|
2017-12-06 01:39:18 -04:00
|
|
|
if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
|
2017-02-07 20:35:31 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
float tune_rp = 0.0f;
|
|
|
|
float tune_rd = 0.0f;
|
|
|
|
float tune_sp = 0.0f;
|
|
|
|
float tune_accel = 0.0f;
|
2017-12-06 01:39:18 -04:00
|
|
|
char axis_char = '?';
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2017-02-07 20:35:31 -04:00
|
|
|
tune_rp = tune_roll_rp;
|
|
|
|
tune_rd = tune_roll_rd;
|
|
|
|
tune_sp = tune_roll_sp;
|
|
|
|
tune_accel = tune_roll_accel;
|
2017-12-06 01:39:18 -04:00
|
|
|
axis_char = 'R';
|
2017-02-07 20:35:31 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2017-02-07 20:35:31 -04:00
|
|
|
tune_rp = tune_pitch_rp;
|
|
|
|
tune_rd = tune_pitch_rd;
|
|
|
|
tune_sp = tune_pitch_sp;
|
|
|
|
tune_accel = tune_pitch_accel;
|
2017-12-06 01:39:18 -04:00
|
|
|
axis_char = 'P';
|
2017-02-07 20:35:31 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2017-02-07 20:35:31 -04:00
|
|
|
tune_rp = tune_yaw_rp;
|
|
|
|
tune_rd = tune_yaw_rLPF;
|
|
|
|
tune_sp = tune_yaw_sp;
|
|
|
|
tune_accel = tune_yaw_accel;
|
2017-12-06 01:39:18 -04:00
|
|
|
axis_char = 'Y';
|
2017-02-07 20:35:31 -04:00
|
|
|
break;
|
|
|
|
}
|
2017-05-16 02:20:38 -03:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis_char, type_string());
|
|
|
|
send_step_string();
|
2017-05-16 02:20:38 -03:00
|
|
|
if (!is_zero(lean_angle)) {
|
2017-12-06 01:39:18 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", (double)lean_angle, (double)target_angle);
|
2017-05-16 02:20:38 -03:00
|
|
|
}
|
|
|
|
if (!is_zero(rotation_rate)) {
|
2017-12-06 01:39:18 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", (double)(rotation_rate*0.01f), (double)(target_rate*0.01f));
|
2017-05-16 02:20:38 -03:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (tune_type) {
|
|
|
|
case RD_UP:
|
|
|
|
case RD_DOWN:
|
|
|
|
case RP_UP:
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
|
2017-02-07 20:35:31 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_DOWN:
|
|
|
|
case SP_UP:
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel);
|
2017-02-07 20:35:31 -04:00
|
|
|
break;
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT);
|
2017-02-07 20:35:31 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
announce_time = now;
|
2017-02-07 20:35:31 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// run - runs the autotune flight mode
|
2014-02-02 03:57:34 -04:00
|
|
|
// should be called at 100hz or more
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::run()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2014-12-03 21:25:42 -04:00
|
|
|
float target_roll, target_pitch;
|
2014-02-02 03:57:34 -04:00
|
|
|
float target_yaw_rate;
|
|
|
|
int16_t target_climb_rate;
|
|
|
|
|
2017-02-07 20:35:31 -04:00
|
|
|
// tell the user what's going on
|
2017-12-06 01:39:18 -04:00
|
|
|
do_gcs_announcements();
|
2017-02-07 20:35:31 -04:00
|
|
|
|
2015-11-18 09:07:42 -04:00
|
|
|
// initialize vertical speeds and acceleration
|
2017-11-08 09:25:53 -04:00
|
|
|
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_accel_z(g.pilot_accel_z);
|
2015-10-19 21:05:10 -03:00
|
|
|
|
2015-04-17 14:49:08 -03:00
|
|
|
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
2017-12-06 01:39:18 -04:00
|
|
|
// this should not actually be possible because of the init() checks
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
|
|
|
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
|
|
|
pos_control->relax_alt_hold_controllers(0.0f);
|
2014-02-02 03:57:34 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
|
|
update_simple_mode();
|
|
|
|
|
|
|
|
// get pilot desired lean angles
|
2016-03-22 20:36:52 -03:00
|
|
|
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// get pilot's desired yaw rate
|
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
|
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// get pilot desired climb rate
|
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
|
|
|
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-01-05 02:24:02 -04:00
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// check for pilot requested take-off - this should not actually be possible because of init() checks
|
2014-02-02 03:57:34 -04:00
|
|
|
if (ap.land_complete && target_climb_rate > 0) {
|
|
|
|
// indicate we are taking off
|
|
|
|
set_land_complete(false);
|
|
|
|
// clear i term when we're taking off
|
|
|
|
set_throttle_takeoff();
|
|
|
|
}
|
|
|
|
|
|
|
|
// reset target lean angles and heading while landed
|
|
|
|
if (ap.land_complete) {
|
2016-07-31 23:12:16 -03:00
|
|
|
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
|
|
|
|
if (target_climb_rate < 0.0f) {
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
2016-07-31 23:12:16 -03:00
|
|
|
} else {
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-17 23:53:02 -04:00
|
|
|
}
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
|
|
|
attitude_control->set_yaw_target_to_current_heading();
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
|
|
|
pos_control->relax_alt_hold_controllers(0.0f);
|
|
|
|
pos_control->update_z_controller();
|
2014-02-02 03:57:34 -04:00
|
|
|
}else{
|
|
|
|
// check if pilot is overriding the controls
|
2017-01-02 21:45:35 -04:00
|
|
|
bool zero_rp_input = is_zero(target_roll) && is_zero(target_pitch);
|
|
|
|
if (!zero_rp_input || !is_zero(target_yaw_rate) || target_climb_rate != 0) {
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!pilot_override) {
|
|
|
|
pilot_override = true;
|
2014-02-02 03:57:34 -04:00
|
|
|
// set gains to their original values
|
2017-12-06 01:39:18 -04:00
|
|
|
load_orig_gains();
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->use_ff_and_input_shaping(true);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
// reset pilot override time
|
2017-12-06 01:39:18 -04:00
|
|
|
override_time = millis();
|
2017-05-22 20:24:14 -03:00
|
|
|
if (!zero_rp_input) {
|
|
|
|
// only reset position on roll or pitch input
|
2017-12-06 01:39:18 -04:00
|
|
|
have_position = false;
|
2017-05-22 20:24:14 -03:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
}else if (pilot_override) {
|
2014-02-02 03:57:34 -04:00
|
|
|
// check if we should resume tuning after pilot's override
|
2017-12-06 01:39:18 -04:00
|
|
|
if (millis() - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) {
|
|
|
|
pilot_override = false; // turn off pilot override
|
2014-02-02 03:57:34 -04:00
|
|
|
// set gains to their intra-test values (which are very close to the original gains)
|
2017-12-06 01:39:18 -04:00
|
|
|
// load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly
|
|
|
|
step = WAITING_FOR_LEVEL; // set tuning step back from beginning
|
|
|
|
desired_yaw = ahrs.yaw_sensor;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-01-02 21:45:35 -04:00
|
|
|
if (zero_rp_input) {
|
|
|
|
// pilot input on throttle and yaw will still use position hold if enabled
|
2017-12-06 01:39:18 -04:00
|
|
|
get_poshold_attitude(target_roll, target_pitch, desired_yaw);
|
2017-01-02 21:45:35 -04:00
|
|
|
}
|
|
|
|
|
2016-01-17 23:53:02 -04:00
|
|
|
// set motors to full range
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-17 23:53:02 -04:00
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
// if pilot override call attitude controller
|
2017-12-06 01:39:18 -04:00
|
|
|
if (pilot_override || mode != TUNING) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
2014-02-02 03:57:34 -04:00
|
|
|
}else{
|
|
|
|
// somehow get attitude requests from autotuning
|
|
|
|
autotune_attitude_control();
|
|
|
|
}
|
|
|
|
|
|
|
|
// call position controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
|
|
|
pos_control->update_z_controller();
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeAutoTune::check_level(const LEVEL_ISSUE issue, const float current, const float maximum)
|
2017-02-12 21:49:55 -04:00
|
|
|
{
|
|
|
|
if (current > maximum) {
|
2017-12-06 01:39:18 -04:00
|
|
|
level_problem.current = current;
|
|
|
|
level_problem.maximum = maximum;
|
|
|
|
level_problem.issue = issue;
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeAutoTune::currently_level()
|
2017-02-12 21:49:55 -04:00
|
|
|
{
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!check_level(LEVEL_ISSUE_ANGLE_ROLL,
|
|
|
|
labs(ahrs.roll_sensor - roll_cd),
|
|
|
|
AUTOTUNE_LEVEL_ANGLE_CD)) {
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!check_level(LEVEL_ISSUE_ANGLE_PITCH,
|
|
|
|
labs(ahrs.pitch_sensor - pitch_cd),
|
|
|
|
AUTOTUNE_LEVEL_ANGLE_CD)) {
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!check_level(LEVEL_ISSUE_ANGLE_YAW,
|
|
|
|
labs(wrap_180_cd(ahrs.yaw_sensor-(int32_t)desired_yaw)),
|
|
|
|
AUTOTUNE_LEVEL_ANGLE_CD)) {
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!check_level(LEVEL_ISSUE_RATE_ROLL,
|
|
|
|
(ToDeg(ahrs.get_gyro().x) * 100.0f),
|
|
|
|
AUTOTUNE_LEVEL_RATE_RP_CD)) {
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!check_level(LEVEL_ISSUE_RATE_PITCH,
|
|
|
|
(ToDeg(ahrs.get_gyro().y) * 100.0f),
|
|
|
|
AUTOTUNE_LEVEL_RATE_RP_CD)) {
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!check_level(LEVEL_ISSUE_RATE_YAW,
|
|
|
|
(ToDeg(ahrs.get_gyro().z) * 100.0f),
|
|
|
|
AUTOTUNE_LEVEL_RATE_Y_CD)) {
|
2017-02-12 21:49:55 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// attitude_controller - sets attitude control targets during tuning
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::autotune_attitude_control()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2017-02-07 20:35:31 -04:00
|
|
|
rotation_rate = 0.0f; // rotation rate in radians/second
|
|
|
|
lean_angle = 0.0f;
|
2017-12-06 01:39:18 -04:00
|
|
|
const float direction_sign = positive_direction ? 1.0f : -1.0f;
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// check tuning step
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (step) {
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
case WAITING_FOR_LEVEL:
|
2014-02-02 03:57:34 -04:00
|
|
|
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
|
|
|
|
// re-enable rate limits
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->use_ff_and_input_shaping(true);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
get_poshold_attitude(roll_cd, pitch_cd, desired_yaw);
|
2017-01-01 00:52:07 -04:00
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
// hold level attitude
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true, get_smoothing_gain());
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2015-04-14 09:58:57 -03:00
|
|
|
// hold the copter level for 0.5 seconds before we begin a twitch
|
2014-02-02 03:57:34 -04:00
|
|
|
// reset counter if we are no longer level
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!currently_level()) {
|
|
|
|
step_start_time = millis();
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2015-04-14 09:58:57 -03:00
|
|
|
// if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step
|
2017-12-06 01:39:18 -04:00
|
|
|
if (millis() - step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) {
|
2017-07-09 00:47:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch");
|
2014-12-05 08:38:28 -04:00
|
|
|
// initiate variables for next step
|
2017-12-06 01:39:18 -04:00
|
|
|
step = TWITCHING;
|
|
|
|
step_start_time = millis();
|
|
|
|
step_stop_time = step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
|
|
|
|
twitch_first_iter = true;
|
|
|
|
test_max = 0.0f;
|
|
|
|
test_min = 0.0f;
|
2015-04-14 09:58:57 -03:00
|
|
|
rotation_rate_filt.reset(0.0f);
|
2014-12-05 08:38:28 -04:00
|
|
|
rate_max = 0.0f;
|
2014-02-02 03:57:34 -04:00
|
|
|
// set gains to their to-be-tested values
|
2017-12-06 01:39:18 -04:00
|
|
|
load_twitch_gains();
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
|
|
|
|
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
|
|
|
start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f;
|
|
|
|
start_angle = ahrs.roll_sensor;
|
2017-01-09 03:31:26 -04:00
|
|
|
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f);
|
2017-12-06 01:39:18 -04:00
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
|
|
|
rotation_rate_filt.reset(start_rate);
|
2015-04-14 09:58:57 -03:00
|
|
|
} else {
|
|
|
|
rotation_rate_filt.reset(0);
|
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
|
|
|
|
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
|
|
|
start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f;
|
|
|
|
start_angle = ahrs.pitch_sensor;
|
2017-01-09 03:31:26 -04:00
|
|
|
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f);
|
2017-12-06 01:39:18 -04:00
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
|
|
|
rotation_rate_filt.reset(start_rate);
|
2015-04-14 09:58:57 -03:00
|
|
|
} else {
|
|
|
|
rotation_rate_filt.reset(0);
|
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS);
|
|
|
|
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
|
|
|
|
start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f;
|
|
|
|
start_angle = ahrs.yaw_sensor;
|
2015-04-16 16:34:25 -03:00
|
|
|
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
|
2017-12-06 01:39:18 -04:00
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
|
|
|
rotation_rate_filt.reset(start_rate);
|
2015-04-14 09:58:57 -03:00
|
|
|
} else {
|
|
|
|
rotation_rate_filt.reset(0);
|
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
break;
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
case TWITCHING:
|
2014-02-02 03:57:34 -04:00
|
|
|
// Run the twitching step
|
|
|
|
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
|
|
|
|
|
|
|
|
// disable rate limits
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->use_ff_and_input_shaping(false);
|
2017-05-25 02:03:36 -03:00
|
|
|
// hold current attitude
|
|
|
|
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
2017-05-25 02:03:36 -03:00
|
|
|
// step angle targets on first iteration
|
2017-12-06 01:39:18 -04:00
|
|
|
if (twitch_first_iter) {
|
|
|
|
twitch_first_iter = false;
|
2017-05-25 02:03:36 -03:00
|
|
|
// Testing increasing stabilize P gain so will set lean angle target
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2017-05-25 02:03:36 -03:00
|
|
|
// request roll to 20deg
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->input_angle_step_bf_roll_pitch_yaw(direction_sign * target_angle, 0.0f, 0.0f);
|
2017-05-25 02:03:36 -03:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2017-05-25 02:03:36 -03:00
|
|
|
// request pitch to 20deg
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, direction_sign * target_angle, 0.0f);
|
2017-05-25 02:03:36 -03:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2017-05-25 02:03:36 -03:00
|
|
|
// request pitch to 20deg
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, direction_sign * target_angle);
|
2017-05-25 02:03:36 -03:00
|
|
|
break;
|
|
|
|
}
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
} else {
|
2014-12-05 08:38:28 -04:00
|
|
|
// Testing rate P and D gains so will set body-frame rate targets.
|
|
|
|
// Rate controller will use existing body-frame rates and convert to motor outputs
|
|
|
|
// for all axes except the one we override here.
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2015-03-11 02:32:44 -03:00
|
|
|
// override body-frame roll rate
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->rate_bf_roll_target(direction_sign * target_rate + start_rate);
|
2015-03-11 02:32:44 -03:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2015-03-11 02:32:44 -03:00
|
|
|
// override body-frame pitch rate
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->rate_bf_pitch_target(direction_sign * target_rate + start_rate);
|
2015-03-11 02:32:44 -03:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2015-03-11 02:32:44 -03:00
|
|
|
// override body-frame yaw rate
|
2017-12-06 01:39:18 -04:00
|
|
|
attitude_control->rate_bf_yaw_target(direction_sign * target_rate + start_rate);
|
2015-03-11 02:32:44 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// capture this iterations rotation rate and lean angle
|
2014-12-05 08:38:28 -04:00
|
|
|
// Add filter to measurements
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
2015-04-16 16:34:25 -03:00
|
|
|
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), MAIN_LOOP_SECONDS);
|
2015-03-11 02:32:44 -03:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), MAIN_LOOP_SECONDS);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)start_angle);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
2015-04-16 16:34:25 -03:00
|
|
|
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), MAIN_LOOP_SECONDS);
|
2015-03-11 02:32:44 -03:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), MAIN_LOOP_SECONDS);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)start_angle);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
2015-04-16 16:34:25 -03:00
|
|
|
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), MAIN_LOOP_SECONDS);
|
2015-03-11 02:32:44 -03:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), MAIN_LOOP_SECONDS);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)start_angle);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (tune_type) {
|
|
|
|
case RD_UP:
|
|
|
|
case RD_DOWN:
|
|
|
|
twitching_test(rotation_rate, target_rate, test_min, test_max);
|
|
|
|
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
|
|
|
|
if (lean_angle >= target_angle) {
|
|
|
|
step = UPDATE_GAINS;
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case RP_UP:
|
|
|
|
twitching_test(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_min, test_max);
|
|
|
|
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
|
|
|
|
if (lean_angle >= target_angle) {
|
|
|
|
step = UPDATE_GAINS;
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_DOWN:
|
|
|
|
case SP_UP:
|
|
|
|
twitching_test(lean_angle, target_angle*(1+0.5f*g.autotune_aggressiveness), test_min, test_max);
|
|
|
|
twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// log this iterations lean angle and rotation rate
|
|
|
|
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
|
2016-03-22 20:36:52 -03:00
|
|
|
_copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
case UPDATE_GAINS:
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// re-enable rate limits
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->use_ff_and_input_shaping(true);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// log the latest gains
|
2017-12-06 01:39:18 -04:00
|
|
|
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
|
2015-03-11 02:32:44 -03:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
|
2015-03-11 02:32:44 -03:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
|
2015-03-11 02:32:44 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Check results after mini-step to increase rate D gain
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (tune_type) {
|
|
|
|
case RD_UP:
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
// Check results after mini-step to decrease rate D gain
|
2017-12-06 01:39:18 -04:00
|
|
|
case RD_DOWN:
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
// Check results after mini-step to increase rate P gain
|
2017-12-06 01:39:18 -04:00
|
|
|
case RP_UP:
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
// Check results after mini-step to increase stabilize P gain
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_DOWN:
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
updating_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
updating_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
updating_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
// Check results after mini-step to increase stabilize P gain
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_UP:
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
|
|
|
updating_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
|
|
|
updating_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
|
|
|
updating_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2014-12-05 08:38:28 -04:00
|
|
|
// we've complete this step, finalize pids and move to next step
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter >= AUTOTUNE_SUCCESS_COUNT) {
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// reset counter
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = 0;
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// move to the next tuning type
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (tune_type) {
|
|
|
|
case RD_UP:
|
|
|
|
tune_type = TuneType(tune_type + 1);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case RD_DOWN:
|
|
|
|
tune_type = TuneType(tune_type + 1);
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_roll_rd = MAX(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
|
|
|
|
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_pitch_rd = MAX(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
|
|
|
|
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
|
|
|
|
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case RP_UP:
|
|
|
|
tune_type = TuneType(tune_type + 1);
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_DOWN:
|
|
|
|
tune_type = TuneType(tune_type + 1);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case SP_UP:
|
2014-02-02 03:57:34 -04:00
|
|
|
// we've reached the end of a D-up-down PI-up-down tune type cycle
|
2017-12-06 01:39:18 -04:00
|
|
|
tune_type = RD_UP;
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2014-12-05 08:38:28 -04:00
|
|
|
// advance to the next axis
|
2017-12-06 01:39:18 -04:00
|
|
|
bool complete = false;
|
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF);
|
2017-12-06 01:39:18 -04:00
|
|
|
tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
|
|
|
|
if (pitch_enabled()) {
|
|
|
|
axis = PITCH;
|
|
|
|
} else if (yaw_enabled()) {
|
|
|
|
axis = YAW;
|
2014-12-05 08:38:28 -04:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
complete = true;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
|
2017-12-06 01:39:18 -04:00
|
|
|
tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
|
|
|
|
if (yaw_enabled()) {
|
|
|
|
axis = YAW;
|
2014-12-05 08:38:28 -04:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
complete = true;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2015-11-27 13:11:58 -04:00
|
|
|
tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
|
2017-12-06 01:39:18 -04:00
|
|
|
tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
|
|
|
|
complete = true;
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// if we've just completed all axes we have successfully completed the autotune
|
2014-02-02 03:57:34 -04:00
|
|
|
// change to TESTING mode to allow user to fly with new gains
|
2017-12-06 01:39:18 -04:00
|
|
|
if (complete) {
|
|
|
|
mode = SUCCESS;
|
|
|
|
update_gcs(AUTOTUNE_MESSAGE_SUCCESS);
|
2014-02-02 03:57:34 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_SUCCESS);
|
2014-10-23 15:47:00 -03:00
|
|
|
AP_Notify::events.autotune_complete = 1;
|
2014-12-05 08:38:28 -04:00
|
|
|
} else {
|
|
|
|
AP_Notify::events.autotune_next_axis = 1;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-12-05 08:38:28 -04:00
|
|
|
// reverse direction
|
2017-12-06 01:39:18 -04:00
|
|
|
positive_direction = !positive_direction;
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (axis == YAW) {
|
2017-06-20 22:57:57 -03:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain());
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// set gains to their intra-test values (which are very close to the original gains)
|
2017-12-06 01:39:18 -04:00
|
|
|
load_intra_test_gains();
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
// reset testing step
|
2017-12-06 01:39:18 -04:00
|
|
|
step = WAITING_FOR_LEVEL;
|
|
|
|
step_start_time = millis();
|
2014-02-02 03:57:34 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// backup_gains_and_initialise - store current gains as originals
|
2014-02-02 03:57:34 -04:00
|
|
|
// called before tuning starts to backup original gains
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::backup_gains_and_initialise()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
|
|
|
// initialise state because this is our first time
|
2017-12-06 01:39:18 -04:00
|
|
|
if (roll_enabled()) {
|
|
|
|
axis = ROLL;
|
|
|
|
} else if (pitch_enabled()) {
|
|
|
|
axis = PITCH;
|
|
|
|
} else if (yaw_enabled()) {
|
|
|
|
axis = YAW;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
positive_direction = false;
|
|
|
|
step = WAITING_FOR_LEVEL;
|
|
|
|
step_start_time = millis();
|
|
|
|
tune_type = RD_UP;
|
2015-04-14 09:58:57 -03:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
desired_yaw = ahrs.yaw_sensor;
|
2015-04-14 09:58:57 -03:00
|
|
|
|
2015-06-18 09:19:44 -03:00
|
|
|
g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05f, 0.2f);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
orig_bf_feedforward = attitude_control->get_bf_feedforward();
|
2015-06-18 09:17:37 -03:00
|
|
|
|
2014-12-05 08:38:28 -04:00
|
|
|
// backup original pids and initialise tuned pid values
|
2017-01-09 03:31:26 -04:00
|
|
|
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
|
|
|
|
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
|
|
|
|
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
|
|
|
|
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
|
|
|
|
orig_roll_accel = attitude_control->get_accel_roll_max();
|
|
|
|
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
|
|
|
|
tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
|
|
|
|
tune_roll_sp = attitude_control->get_angle_roll_p().kP();
|
|
|
|
tune_roll_accel = attitude_control->get_accel_roll_max();
|
|
|
|
|
|
|
|
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
|
|
|
|
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
|
|
|
|
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
|
|
|
|
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
|
|
|
|
orig_pitch_accel = attitude_control->get_accel_pitch_max();
|
|
|
|
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
|
|
|
|
tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
|
|
|
|
tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
|
|
|
|
tune_pitch_accel = attitude_control->get_accel_pitch_max();
|
|
|
|
|
|
|
|
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
|
|
|
|
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
|
|
|
|
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
|
|
|
|
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
|
|
|
|
orig_yaw_accel = attitude_control->get_accel_yaw_max();
|
|
|
|
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
|
|
|
|
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
|
|
|
|
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
|
|
|
|
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
|
|
|
|
tune_yaw_accel = attitude_control->get_accel_yaw_max();
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
Log_Write_Event(DATA_AUTOTUNE_INITIALISED);
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// load_orig_gains - set gains to their original values
|
|
|
|
// called by stop and failed functions
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::load_orig_gains()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->bf_feedforward(orig_bf_feedforward);
|
2017-12-06 01:39:18 -04:00
|
|
|
if (roll_enabled()) {
|
2015-06-05 05:38:25 -03:00
|
|
|
if (!is_zero(orig_roll_rp)) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
|
|
|
|
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
|
|
|
|
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
|
|
|
|
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
|
|
|
|
attitude_control->set_accel_roll_max(orig_roll_accel);
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (pitch_enabled()) {
|
2015-06-05 05:38:25 -03:00
|
|
|
if (!is_zero(orig_pitch_rp)) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
|
|
|
|
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
|
|
|
|
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
|
|
|
|
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
|
|
|
|
attitude_control->set_accel_pitch_max(orig_pitch_accel);
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (yaw_enabled()) {
|
2015-06-05 05:38:25 -03:00
|
|
|
if (!is_zero(orig_yaw_rp)) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
|
|
|
|
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri);
|
|
|
|
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
|
|
|
|
attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
|
|
|
|
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
|
|
|
|
attitude_control->set_accel_yaw_max(orig_yaw_accel);
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// load_tuned_gains - load tuned gains
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::load_tuned_gains()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!attitude_control->get_bf_feedforward()) {
|
|
|
|
attitude_control->bf_feedforward(true);
|
|
|
|
attitude_control->set_accel_roll_max(0.0f);
|
|
|
|
attitude_control->set_accel_pitch_max(0.0f);
|
2015-06-18 09:17:37 -03:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (roll_enabled()) {
|
2015-05-04 23:34:21 -03:00
|
|
|
if (!is_zero(tune_roll_rp)) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
|
|
|
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
|
|
|
|
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
|
|
|
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
|
|
|
attitude_control->set_accel_roll_max(tune_roll_accel);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (pitch_enabled()) {
|
2015-05-04 23:34:21 -03:00
|
|
|
if (!is_zero(tune_pitch_rp)) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
|
|
|
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
|
|
|
|
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
|
|
|
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
|
|
|
attitude_control->set_accel_pitch_max(tune_pitch_accel);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (yaw_enabled()) {
|
2015-05-04 23:34:21 -03:00
|
|
|
if (!is_zero(tune_yaw_rp)) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
|
|
|
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
|
|
|
|
attitude_control->get_rate_yaw_pid().kD(0.0f);
|
|
|
|
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
|
|
|
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
|
|
|
attitude_control->set_accel_yaw_max(tune_yaw_accel);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// load_intra_test_gains - gains used between tests
|
2014-02-02 03:57:34 -04:00
|
|
|
// called during testing mode's update-gains step to set gains ahead of return-to-level step
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::load_intra_test_gains()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
|
|
|
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
|
2014-12-05 08:38:28 -04:00
|
|
|
// sanity check the gains
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->bf_feedforward(true);
|
2017-12-06 01:39:18 -04:00
|
|
|
if (roll_enabled()) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
|
|
|
|
attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
|
|
|
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
|
|
|
|
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (pitch_enabled()) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
|
|
|
|
attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
|
|
|
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
|
|
|
|
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2017-12-06 01:39:18 -04:00
|
|
|
if (yaw_enabled()) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
|
|
|
|
attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
|
|
|
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
|
|
|
|
attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
|
|
|
|
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// load_twitch_gains - load the to-be-tested gains for a single axis
|
|
|
|
// called by attitude_control() just before it beings testing a gain (i.e. just before it twitches)
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::load_twitch_gains()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2017-12-06 01:39:18 -04:00
|
|
|
switch (axis) {
|
|
|
|
case ROLL:
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
|
|
|
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f);
|
|
|
|
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
|
|
|
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case PITCH:
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
|
|
|
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
|
|
|
|
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
|
|
|
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
2017-12-06 01:39:18 -04:00
|
|
|
case YAW:
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
|
|
|
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
|
|
|
|
attitude_control->get_rate_yaw_pid().kD(0.0f);
|
|
|
|
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
|
|
|
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// save_tuning_gains - save the final tuned gains for each axis
|
2014-02-02 03:57:34 -04:00
|
|
|
// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::save_tuning_gains()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
|
|
|
// if we successfully completed tuning
|
2017-12-06 01:39:18 -04:00
|
|
|
if (mode == SUCCESS) {
|
2015-05-06 04:10:18 -03:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!attitude_control->get_bf_feedforward()) {
|
|
|
|
attitude_control->bf_feedforward_save(true);
|
|
|
|
attitude_control->save_accel_roll_max(0.0f);
|
|
|
|
attitude_control->save_accel_pitch_max(0.0f);
|
2015-05-06 04:10:18 -03:00
|
|
|
}
|
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
// sanity check the rate P values
|
2017-12-06 01:39:18 -04:00
|
|
|
if (roll_enabled() && !is_zero(tune_roll_rp)) {
|
2014-02-02 03:57:34 -04:00
|
|
|
// rate roll gains
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
|
|
|
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
|
|
|
|
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
|
|
|
attitude_control->get_rate_roll_pid().save_gains();
|
2015-05-06 04:10:18 -03:00
|
|
|
|
2015-03-11 02:32:44 -03:00
|
|
|
// stabilize roll
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
|
|
|
attitude_control->get_angle_roll_p().save_gains();
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2015-05-06 04:10:18 -03:00
|
|
|
// acceleration roll
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->save_accel_roll_max(tune_roll_accel);
|
2015-03-11 02:32:44 -03:00
|
|
|
|
|
|
|
// resave pids to originals in case the autotune is run again
|
2017-01-09 03:31:26 -04:00
|
|
|
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
|
|
|
|
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
|
|
|
|
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
|
|
|
|
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
|
2017-06-21 06:05:45 -03:00
|
|
|
orig_roll_accel = attitude_control->get_accel_roll_max();
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (pitch_enabled() && !is_zero(tune_pitch_rp)) {
|
2014-02-02 03:57:34 -04:00
|
|
|
// rate pitch gains
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
|
|
|
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
|
|
|
|
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
|
|
|
attitude_control->get_rate_pitch_pid().save_gains();
|
2015-05-06 04:10:18 -03:00
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
// stabilize pitch
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
|
|
|
attitude_control->get_angle_pitch_p().save_gains();
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2015-05-06 04:10:18 -03:00
|
|
|
// acceleration pitch
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->save_accel_pitch_max(tune_pitch_accel);
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// resave pids to originals in case the autotune is run again
|
2017-01-09 03:31:26 -04:00
|
|
|
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
|
|
|
|
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
|
|
|
|
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
|
|
|
|
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
|
2017-06-21 06:05:45 -03:00
|
|
|
orig_pitch_accel = attitude_control->get_accel_pitch_max();
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (yaw_enabled() && !is_zero(tune_yaw_rp)) {
|
2015-03-11 02:32:44 -03:00
|
|
|
// rate yaw gains
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
|
|
|
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
|
|
|
|
attitude_control->get_rate_yaw_pid().kD(0.0f);
|
|
|
|
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
|
|
|
attitude_control->get_rate_yaw_pid().save_gains();
|
2015-05-06 04:10:18 -03:00
|
|
|
|
2015-03-11 02:32:44 -03:00
|
|
|
// stabilize yaw
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
|
|
|
attitude_control->get_angle_yaw_p().save_gains();
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2015-05-06 04:10:18 -03:00
|
|
|
// acceleration yaw
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->save_accel_yaw_max(tune_yaw_accel);
|
2015-03-11 02:32:44 -03:00
|
|
|
|
|
|
|
// resave pids to originals in case the autotune is run again
|
2017-01-09 03:31:26 -04:00
|
|
|
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
|
|
|
|
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
|
|
|
|
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
|
|
|
|
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
|
|
|
|
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
|
2017-06-21 06:05:45 -03:00
|
|
|
orig_yaw_accel = attitude_control->get_accel_pitch_max();
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
// update GCS and log save gains event
|
2017-12-06 01:39:18 -04:00
|
|
|
update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
|
2015-03-21 10:55:39 -03:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS);
|
|
|
|
// reset Autotune so that gains are not saved again and autotune can be run again.
|
2017-12-06 01:39:18 -04:00
|
|
|
mode = UNINITIALISED;
|
2015-03-11 02:32:44 -03:00
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// update_gcs - send message to ground station
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::update_gcs(uint8_t message_id)
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
|
|
|
switch (message_id) {
|
|
|
|
case AUTOTUNE_MESSAGE_STARTED:
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Started");
|
2014-02-02 03:57:34 -04:00
|
|
|
break;
|
|
|
|
case AUTOTUNE_MESSAGE_STOPPED:
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped");
|
2014-02-02 03:57:34 -04:00
|
|
|
break;
|
|
|
|
case AUTOTUNE_MESSAGE_SUCCESS:
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Success");
|
2014-02-02 03:57:34 -04:00
|
|
|
break;
|
|
|
|
case AUTOTUNE_MESSAGE_FAILED:
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed");
|
2014-02-02 03:57:34 -04:00
|
|
|
break;
|
2014-12-05 08:38:28 -04:00
|
|
|
case AUTOTUNE_MESSAGE_SAVED_GAINS:
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Saved gains");
|
2014-12-05 08:38:28 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// axis helper functions
|
2017-12-10 23:51:13 -04:00
|
|
|
inline bool Copter::ModeAutoTune::roll_enabled() {
|
2014-12-05 08:38:28 -04:00
|
|
|
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL;
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
inline bool Copter::ModeAutoTune::pitch_enabled() {
|
2014-12-05 08:38:28 -04:00
|
|
|
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH;
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
inline bool Copter::ModeAutoTune::yaw_enabled() {
|
2014-12-05 08:38:28 -04:00
|
|
|
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// twitching_test - twitching tests
|
2015-03-22 08:23:11 -03:00
|
|
|
// update min and max and test for end conditions
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::twitching_test(float measurement, float target, float &measurement_min, float &measurement_max)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
2015-03-07 04:33:16 -04:00
|
|
|
// capture maximum measurement
|
|
|
|
if (measurement > measurement_max) {
|
|
|
|
// the measurement is continuing to increase without stopping
|
|
|
|
measurement_max = measurement;
|
|
|
|
measurement_min = measurement;
|
|
|
|
}
|
|
|
|
|
|
|
|
// capture minimum measurement after the measurement has peaked (aka "bounce back")
|
|
|
|
if ((measurement < measurement_min) && (measurement_max > target * 0.5f)) {
|
|
|
|
// the measurement is bouncing back
|
|
|
|
measurement_min = measurement;
|
|
|
|
}
|
|
|
|
|
|
|
|
// calculate early stopping time based on the time it takes to get to 90%
|
2015-05-06 02:27:37 -03:00
|
|
|
if (measurement_max < target * 0.75f) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// the measurement not reached the 90% threshold yet
|
2017-12-06 01:39:18 -04:00
|
|
|
step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f;
|
|
|
|
step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
if (measurement_max > target) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// the measurement has passed the target
|
2017-12-06 01:39:18 -04:00
|
|
|
step = UPDATE_GAINS;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
|
2015-03-07 04:33:16 -04:00
|
|
|
if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) {
|
|
|
|
// the measurement has passed 50% of the target and bounce back is larger than the threshold
|
2017-12-06 01:39:18 -04:00
|
|
|
step = UPDATE_GAINS;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (millis() >= step_stop_time) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// we have passed the maximum stop time
|
2017-12-06 01:39:18 -04:00
|
|
|
step = UPDATE_GAINS;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// updating_d_up - increase D and adjust P to optimize the D term for a little bounce back
|
2015-03-07 04:33:16 -04:00
|
|
|
// optimize D term while keeping the maximum just below the target by adjusting P
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
|
|
|
if (measurement_max > target) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// if maximum measurement was higher than target
|
|
|
|
// reduce P gain (which should reduce maximum)
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p -= tune_p*tune_p_step_ratio;
|
2015-03-11 02:32:44 -03:00
|
|
|
if (tune_p < tune_p_min) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// P gain is at minimum so start reducing D
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p = tune_p_min;
|
|
|
|
tune_d -= tune_d*tune_d_step_ratio;
|
|
|
|
if (tune_d <= tune_d_min) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// We have reached minimum D gain so stop tuning
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_d = tune_d_min;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2014-12-05 08:38:28 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
}
|
2015-03-11 02:32:44 -03:00
|
|
|
}else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// we have not achieved a high enough maximum to get a good measurement of bounce back.
|
|
|
|
// increase P gain (which should increase maximum)
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p += tune_p*tune_p_step_ratio;
|
2015-04-15 09:39:12 -03:00
|
|
|
if (tune_p >= tune_p_max) {
|
|
|
|
tune_p = tune_p_max;
|
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}else{
|
2015-03-07 04:33:16 -04:00
|
|
|
// we have a good measurement of bounce back
|
2014-12-05 08:38:28 -04:00
|
|
|
if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// ignore the next result unless it is the same as this one
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = true;
|
2015-03-07 04:33:16 -04:00
|
|
|
// bounce back is bigger than our threshold so increment the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
counter++;
|
2014-12-05 08:38:28 -04:00
|
|
|
}else{
|
2017-12-06 01:39:18 -04:00
|
|
|
if (ignore_next == false) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// bounce back is smaller than our threshold so decrement the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter > 0 ) {
|
|
|
|
counter--;
|
2015-04-14 09:58:57 -03:00
|
|
|
}
|
|
|
|
// increase D gain (which should increase bounce back)
|
|
|
|
tune_d += tune_d*tune_d_step_ratio*2.0f;
|
|
|
|
// stop tuning if we hit maximum D
|
|
|
|
if (tune_d >= tune_d_max) {
|
|
|
|
tune_d = tune_d_max;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2015-04-14 09:58:57 -03:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = false;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// updating_d_down - decrease D and adjust P to optimize the D term for no bounce back
|
2015-03-07 04:33:16 -04:00
|
|
|
// optimize D term while keeping the maximum just below the target by adjusting P
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
|
|
|
if (measurement_max > target) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// if maximum measurement was higher than target
|
|
|
|
// reduce P gain (which should reduce maximum)
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p -= tune_p*tune_p_step_ratio;
|
2015-03-11 02:32:44 -03:00
|
|
|
if (tune_p < tune_p_min) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// P gain is at minimum so start reducing D gain
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p = tune_p_min;
|
|
|
|
tune_d -= tune_d*tune_d_step_ratio;
|
|
|
|
if (tune_d <= tune_d_min) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// We have reached minimum D so stop tuning
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_d = tune_d_min;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2014-12-05 08:38:28 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
}
|
2015-03-11 02:32:44 -03:00
|
|
|
}else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// we have not achieved a high enough maximum to get a good measurement of bounce back.
|
|
|
|
// increase P gain (which should increase maximum)
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p += tune_p*tune_p_step_ratio;
|
2015-04-15 09:39:12 -03:00
|
|
|
if (tune_p >= tune_p_max) {
|
|
|
|
tune_p = tune_p_max;
|
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}else{
|
2015-03-07 04:33:16 -04:00
|
|
|
// we have a good measurement of bounce back
|
2014-12-05 08:38:28 -04:00
|
|
|
if (measurement_max-measurement_min < measurement_max*g.autotune_aggressiveness) {
|
2017-12-06 01:39:18 -04:00
|
|
|
if (ignore_next == false) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// bounce back is less than our threshold so increment the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
counter++;
|
2015-04-14 09:58:57 -03:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = false;
|
2015-04-14 09:58:57 -03:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}else{
|
2015-04-14 09:58:57 -03:00
|
|
|
// ignore the next result unless it is the same as this one
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = true;
|
2015-03-07 04:33:16 -04:00
|
|
|
// bounce back is larger than our threshold so decrement the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter > 0 ) {
|
|
|
|
counter--;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2015-03-07 04:33:16 -04:00
|
|
|
// decrease D gain (which should decrease bounce back)
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_d -= tune_d*tune_d_step_ratio;
|
2015-03-07 04:33:16 -04:00
|
|
|
// stop tuning if we hit minimum D
|
2014-12-05 08:38:28 -04:00
|
|
|
if (tune_d <= tune_d_min) {
|
|
|
|
tune_d = tune_d_min;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2014-12-05 08:38:28 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// updating_p_down - decrease P until we don't reach the target before time out
|
2015-03-07 04:33:16 -04:00
|
|
|
// P is decreased to ensure we are not overshooting the target
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
2015-06-07 04:38:02 -03:00
|
|
|
if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) {
|
2017-12-06 01:39:18 -04:00
|
|
|
if (ignore_next == false) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// if maximum measurement was lower than target so increment the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
counter++;
|
2015-04-14 09:58:57 -03:00
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = false;
|
2015-04-14 09:58:57 -03:00
|
|
|
}
|
2014-12-05 08:38:28 -04:00
|
|
|
}else{
|
2015-04-14 09:58:57 -03:00
|
|
|
// ignore the next result unless it is the same as this one
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = true;
|
2015-03-07 04:33:16 -04:00
|
|
|
// if maximum measurement was higher than target so decrement the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter > 0 ) {
|
|
|
|
counter--;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
2015-03-07 04:33:16 -04:00
|
|
|
// decrease P gain (which should decrease the maximum)
|
2014-12-05 08:38:28 -04:00
|
|
|
tune_p -= tune_p*tune_p_step_ratio;
|
2015-03-07 04:33:16 -04:00
|
|
|
// stop tuning if we hit maximum P
|
2014-12-05 08:38:28 -04:00
|
|
|
if (tune_p <= tune_p_min) {
|
|
|
|
tune_p = tune_p_min;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2014-12-05 08:38:28 -04:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// updating_p_up - increase P to ensure the target is reached
|
2015-03-07 04:33:16 -04:00
|
|
|
// P is increased until we achieve our target within a reasonable time
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max)
|
2015-03-07 04:33:16 -04:00
|
|
|
{
|
2015-06-07 04:38:02 -03:00
|
|
|
if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// ignore the next result unless it is the same as this one
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = 1;
|
2015-03-07 04:33:16 -04:00
|
|
|
// if maximum measurement was greater than target so increment the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
counter++;
|
2015-03-07 04:33:16 -04:00
|
|
|
}else{
|
2017-12-06 01:39:18 -04:00
|
|
|
if (ignore_next == false) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// if maximum measurement was lower than target so decrement the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter > 0 ) {
|
|
|
|
counter--;
|
2015-04-14 09:58:57 -03:00
|
|
|
}
|
|
|
|
// increase P gain (which should increase the maximum)
|
|
|
|
tune_p += tune_p*tune_p_step_ratio;
|
|
|
|
// stop tuning if we hit maximum P
|
|
|
|
if (tune_p >= tune_p_max) {
|
|
|
|
tune_p = tune_p_max;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2015-04-14 09:58:57 -03:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = false;
|
2015-03-07 04:33:16 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing
|
2015-03-07 04:33:16 -04:00
|
|
|
// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
2015-06-07 04:38:02 -03:00
|
|
|
if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// ignore the next result unless it is the same as this one
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = true;
|
2015-03-07 04:33:16 -04:00
|
|
|
// if maximum measurement was greater than target so increment the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
counter++;
|
2016-08-04 09:30:03 -03:00
|
|
|
} else if ((measurement_max < target) && (measurement_max > target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) {
|
2015-03-07 04:33:16 -04:00
|
|
|
// if bounce back was larger than the threshold so decrement the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter > 0 ) {
|
|
|
|
counter--;
|
2015-03-07 04:33:16 -04:00
|
|
|
}
|
|
|
|
// decrease D gain (which should decrease bounce back)
|
|
|
|
tune_d -= tune_d*tune_d_step_ratio;
|
|
|
|
// stop tuning if we hit minimum D
|
|
|
|
if (tune_d <= tune_d_min) {
|
|
|
|
tune_d = tune_d_min;
|
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
// decrease P gain to match D gain reduction
|
|
|
|
tune_p -= tune_p*tune_p_step_ratio;
|
|
|
|
// stop tuning if we hit minimum P
|
|
|
|
if (tune_p <= tune_p_min) {
|
|
|
|
tune_p = tune_p_min;
|
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
2015-04-14 09:58:57 -03:00
|
|
|
// cancel change in direction
|
2017-12-06 01:39:18 -04:00
|
|
|
positive_direction = !positive_direction;
|
2014-12-05 08:38:28 -04:00
|
|
|
}else{
|
2017-12-06 01:39:18 -04:00
|
|
|
if (ignore_next == false) {
|
2015-04-14 09:58:57 -03:00
|
|
|
// if maximum measurement was lower than target so decrement the success counter
|
2017-12-06 01:39:18 -04:00
|
|
|
if (counter > 0 ) {
|
|
|
|
counter--;
|
2015-04-14 09:58:57 -03:00
|
|
|
}
|
|
|
|
// increase P gain (which should increase the maximum)
|
|
|
|
tune_p += tune_p*tune_p_step_ratio;
|
|
|
|
// stop tuning if we hit maximum P
|
|
|
|
if (tune_p >= tune_p_max) {
|
|
|
|
tune_p = tune_p_max;
|
2017-12-06 01:39:18 -04:00
|
|
|
counter = AUTOTUNE_SUCCESS_COUNT;
|
2015-04-14 09:58:57 -03:00
|
|
|
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
|
|
|
|
}
|
|
|
|
} else {
|
2017-12-06 01:39:18 -04:00
|
|
|
ignore_next = false;
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
// twitching_measure_acceleration - measure rate of change of measurement
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
2015-03-11 02:32:44 -03:00
|
|
|
if (rate_measurement_max < rate_measurement) {
|
2014-12-05 08:38:28 -04:00
|
|
|
rate_measurement_max = rate_measurement;
|
2017-12-06 01:39:18 -04:00
|
|
|
rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time);
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-01-01 00:52:07 -04:00
|
|
|
// get attitude for slow position hold in autotune mode
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out)
|
2017-01-01 00:52:07 -04:00
|
|
|
{
|
2017-12-06 01:39:18 -04:00
|
|
|
roll_cd_out = pitch_cd_out = 0;
|
2017-01-02 21:45:35 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!use_poshold) {
|
2017-01-02 21:45:35 -04:00
|
|
|
// we are not trying to hold position
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// do we know where we are?
|
2016-03-22 20:36:52 -03:00
|
|
|
if (!_copter.position_ok()) {
|
2017-01-02 21:45:35 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
if (!have_position) {
|
|
|
|
have_position = true;
|
|
|
|
start_position = inertial_nav.get_position();
|
2017-01-01 00:52:07 -04:00
|
|
|
}
|
|
|
|
|
2017-01-02 21:45:35 -04:00
|
|
|
// don't go past 10 degrees, as autotune result would deteriorate too much
|
|
|
|
const float angle_max_cd = 1000;
|
|
|
|
|
|
|
|
// hit the 10 degree limit at 20 meters position error
|
|
|
|
const float dist_limit_cm = 2000;
|
2017-01-02 22:57:24 -04:00
|
|
|
|
|
|
|
// we only start adjusting yaw if we are more than 5m from the
|
|
|
|
// target position. That corresponds to a lean angle of 2.5 degrees
|
|
|
|
const float yaw_dist_limit_cm = 500;
|
2017-01-02 21:45:35 -04:00
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
Vector3f pdiff = inertial_nav.get_position() - start_position;
|
2017-01-01 00:52:07 -04:00
|
|
|
pdiff.z = 0;
|
|
|
|
float dist_cm = pdiff.length();
|
|
|
|
if (dist_cm < 10) {
|
|
|
|
// don't do anything within 10cm
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
very simple linear controller
|
|
|
|
*/
|
|
|
|
float scaling = constrain_float(angle_max_cd * dist_cm / dist_limit_cm, 0, angle_max_cd);
|
|
|
|
Vector2f angle_ne(pdiff.x, pdiff.y);
|
|
|
|
angle_ne *= scaling / dist_cm;
|
|
|
|
|
|
|
|
// rotate into body frame
|
2017-12-06 01:39:18 -04:00
|
|
|
pitch_cd_out = angle_ne.x * ahrs.cos_yaw() + angle_ne.y * ahrs.sin_yaw();
|
|
|
|
roll_cd_out = angle_ne.x * ahrs.sin_yaw() - angle_ne.y * ahrs.cos_yaw();
|
2017-01-02 22:57:24 -04:00
|
|
|
|
|
|
|
if (dist_cm < yaw_dist_limit_cm) {
|
|
|
|
// no yaw adjustment
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
also point so that twitching occurs perpendicular to the wind,
|
|
|
|
if we have drifted more than yaw_dist_limit_cm from the desired
|
|
|
|
position. This ensures that autotune doesn't have to deal with
|
|
|
|
more than 2.5 degrees of attitude on the axis it is tuning
|
|
|
|
*/
|
|
|
|
float target_yaw_cd = degrees(atan2f(pdiff.y, pdiff.x)) * 100;
|
2017-12-06 01:39:18 -04:00
|
|
|
if (axis == PITCH) {
|
2017-01-02 22:57:24 -04:00
|
|
|
// for roll and yaw tuning we point along the wind, for pitch
|
|
|
|
// we point across the wind
|
|
|
|
target_yaw_cd += 9000;
|
|
|
|
}
|
|
|
|
// go to the nearest 180 degree mark, with 5 degree slop to prevent oscillation
|
2017-12-06 01:39:18 -04:00
|
|
|
if (fabsf(yaw_cd_out - target_yaw_cd) > 9500) {
|
2017-01-02 22:57:24 -04:00
|
|
|
target_yaw_cd += 18000;
|
|
|
|
}
|
|
|
|
|
2017-12-06 01:39:18 -04:00
|
|
|
yaw_cd_out = target_yaw_cd;
|
2017-01-01 00:52:07 -04:00
|
|
|
}
|
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
#endif // AUTOTUNE_ENABLED == ENABLED
|