AC_AutoTune: formatting and comment improvements

libraries/AC_AutoTune/AC_AutoTune.cpp

@@ -347,6 +347,7 @@ void AC_AutoTune::run()
 
 }
 
+// check if current is greater than maximum and update level_problem structure
 bool AC_AutoTune::check_level(const LevelIssue issue, const float current, const float maximum)
 {
     if (current > maximum) {
@@ -358,6 +359,7 @@ bool AC_AutoTune::check_level(const LevelIssue issue, const float current, const
     return true;
 }
 
+// return true if vehicle is close to level
 bool AC_AutoTune::currently_level()
 {
     float threshold_mul = 1.0;
@@ -410,7 +412,8 @@ bool AC_AutoTune::currently_level()
     return true;
 }
 
-// attitude_controller - sets attitude control targets during tuning
+// main state machine to level vehicle, perform a test and update gains
+// directly updates attitude controller with targets
 void AC_AutoTune::control_attitude()
 {
     rotation_rate = 0.0f;        // rotation rate in radians/second
@@ -509,7 +512,6 @@ void AC_AutoTune::control_attitude()
         attitude_control->use_sqrt_controller(true);
 
         // log the latest gains
-
         Log_AutoTune();
 
         switch (tune_type) {
@@ -570,7 +572,7 @@ void AC_AutoTune::control_attitude()
                     tune_pitch_rp = MAX(get_rp_min(), tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
                     break;
                 case YAW:
-                    tune_yaw_rLPF = MAX(get_rlpf_min(), tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
+                    tune_yaw_rLPF = MAX(get_yaw_rate_filt_min(), tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
                     tune_yaw_rp = MAX(get_rp_min(), tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
                     break;
                 }
@@ -816,7 +818,7 @@ void AC_AutoTune::load_tuned_gains()
     if (roll_enabled()) {
         if (!is_zero(tune_roll_rp) || allow_zero_rate_p()) {
             attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
-            attitude_control->get_rate_roll_pid().kI(get_load_tuned_ri(axis));
+            attitude_control->get_rate_roll_pid().kI(get_tuned_ri(axis));
             attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
             attitude_control->get_rate_roll_pid().ff(tune_roll_rff);
             attitude_control->get_angle_roll_p().kP(tune_roll_sp);
@@ -826,7 +828,7 @@ void AC_AutoTune::load_tuned_gains()
     if (pitch_enabled()) {
         if (!is_zero(tune_pitch_rp) || allow_zero_rate_p()) {
             attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
-            attitude_control->get_rate_pitch_pid().kI(get_load_tuned_ri(axis));
+            attitude_control->get_rate_pitch_pid().kI(get_tuned_ri(axis));
             attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
             attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff);
             attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
@@ -836,8 +838,8 @@ void AC_AutoTune::load_tuned_gains()
     if (yaw_enabled()) {
         if (!is_zero(tune_yaw_rp)) {
             attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
-            attitude_control->get_rate_yaw_pid().kI(get_load_tuned_ri(axis));
+            attitude_control->get_rate_yaw_pid().kI(get_tuned_ri(axis));
-            attitude_control->get_rate_yaw_pid().kD(get_load_tuned_yaw_rd());
+            attitude_control->get_rate_yaw_pid().kD(get_tuned_yaw_rd());
             attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff);
             attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
             attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);

libraries/AC_AutoTune/AC_AutoTune.h

@@ -41,8 +41,6 @@
 
 #define AUTOTUNE_DWELL_CYCLES                10
 
-
-
 class AC_AutoTune
 {
 public:
@@ -68,10 +66,20 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
+
+    // axis that can be tuned
+    enum AxisType {
+        ROLL = 0,                 // roll axis is being tuned (either angle or rate)
+        PITCH = 1,                // pitch axis is being tuned (either angle or rate)
+        YAW = 2,                  // pitch axis is being tuned (either angle or rate)
+    };
+
+    //
     // methods that must be supplied by the vehicle specific subclass
+    //
     virtual bool init(void) = 0;
 
-    // get pilot input for desired cimb rate
+    // get pilot input for desired climb rate
     virtual float get_pilot_desired_climb_rate_cms(void) const = 0;
 
     // get pilot input for designed roll and pitch, and yaw rate
@@ -96,56 +104,105 @@ protected:
     // initialise position controller
     bool init_position_controller();
 
-    // things that can be tuned
+    // main state machine to level vehicle, perform a test and update gains
-    enum AxisType {
+    // directly updates attitude controller with targets
-        ROLL = 0,                 // roll axis is being tuned (either angle or rate)
-        PITCH = 1,                // pitch axis is being tuned (either angle or rate)
-        YAW = 2,                  // pitch axis is being tuned (either angle or rate)
-    };
-
     void control_attitude();
+
+    //
+    // methods to load and save gains
+    //
+
+    // backup original gains and prepare for start of tuning
     void backup_gains_and_initialise();
+
+    // switch to use original gains
     void load_orig_gains();
+
+    // switch to gains found by last successful autotune
     void load_tuned_gains();
+
+    // load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
     void load_intra_test_gains();
+
+    // load gains for next test.  relies on axis variable being set
     virtual void load_test_gains();
+
+    // get intra test rate I gain for the specified axis
+    virtual float get_intra_test_ri(AxisType test_axis) = 0;
+
+    // get tuned rate I gain for the specified axis
+    virtual float get_tuned_ri(AxisType test_axis) = 0;
+
+    // get tuned yaw rate d gain
+    virtual float get_tuned_yaw_rd() = 0;
+
+    // test init and run methods that should be overridden for each vehicle
     virtual void test_init() = 0;
     virtual void test_run(AxisType test_axis, const float dir_sign) = 0;
-    void update_gcs(uint8_t message_id) const;
+
+    // return true if user has enabled autotune for roll, pitch or yaw axis
     bool roll_enabled() const;
     bool pitch_enabled() const;
     bool yaw_enabled() const;
+
     void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max);
     void twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min);
     void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max);
     void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const;
 
-    // Added generic twitch test functions for multi
+    // twitch test functions for multicopter
     void twitch_test_init();
     void twitch_test_run(AxisType test_axis, const float dir_sign);
 
-    // replace multi specific updating gain functions with generic forms that covers all axes
+    // update gains for the rate p up tune type
-    // generic method used by subclasses to update gains for the rate p up tune type
     virtual void updating_rate_p_up_all(AxisType test_axis)=0;
-    // generic method used by subclasses to update gains for the rate p down tune type
+
+    // update gains for the rate p down tune type
     virtual void updating_rate_p_down_all(AxisType test_axis)=0;
-    // generic method used by subclasses to update gains for the rate d up tune type
+
+    // update gains for the rate d up tune type
     virtual void updating_rate_d_up_all(AxisType test_axis)=0;
-    // generic method used by subclasses to update gains for the rate d down tune type
+
+    // update gains for the rate d down tune type
     virtual void updating_rate_d_down_all(AxisType test_axis)=0;
-    // generic method used by subclasses to update gains for the angle p up tune type
+
+    // update gains for the angle p up tune type
     virtual void updating_angle_p_up_all(AxisType test_axis)=0;
-    // generic method used by subclasses to update gains for the angle p down tune type
+
+    // update gains for the angle p down tune type
     virtual void updating_angle_p_down_all(AxisType test_axis)=0;
+
+    // returns true if rate P gain of zero is acceptable for this vehicle
+    virtual bool allow_zero_rate_p() = 0;
+
+    // get minimum rate P (for any axis)
+    virtual float get_rp_min() const = 0;
+
+    // get minimum angle P (for any axis)
+    virtual float get_sp_min() const = 0;
+
+    // get minimum rate Yaw filter value
+    virtual float get_yaw_rate_filt_min() const = 0;
+
+    // get attitude for slow position hold in autotune mode
     void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
 
     virtual void Log_AutoTune() = 0;
     virtual void Log_AutoTuneDetails() = 0;
 
+    // send message with high level status (e.g. Started, Stopped)
+    void update_gcs(uint8_t message_id) const;
+
+    // send lower level step status (e.g. Pilot overrides Active)
     void send_step_string();
+
+    // convert latest level issue to string for reporting
     const char *level_issue_string() const;
-    const char * type_string() const;
+
-    void announce_state_to_gcs();
+    // convert tune type to string for reporting
+    const char *type_string() const;
+
+    // send intermittant updates to user on status of tune
     virtual void do_gcs_announcements() = 0;
 
     enum struct LevelIssue {
@@ -157,7 +214,11 @@ protected:
         RATE_PITCH,
         RATE_YAW,
     };
+
+    // check if current is greater than maximum and update level_problem structure
     bool check_level(const enum LevelIssue issue, const float current, const float maximum);
+
+    // returns true if vehicle is close to level
     bool currently_level();
 
     // autotune modes (high level states)
@@ -188,6 +249,8 @@ protected:
         MAX_GAINS = 8,            // max allowable stable gains are determined
         TUNE_COMPLETE = 9         // Reached end of tuning
     };
+    TuneType tune_seq[6];         // holds sequence of tune_types to be performed
+    uint8_t tune_seq_curr;        // current tune sequence step
 
     // type of gains to load
     enum GainType {
@@ -200,7 +263,7 @@ protected:
 
     TuneMode mode;                       // see TuneMode for what modes are allowed
     bool     pilot_override;             // true = pilot is overriding controls so we suspend tuning temporarily
-    AxisType axis;                       // see AxisType for which things can be tuned
+    AxisType axis;                       // current axis being tuned. see AxisType enum
     bool     positive_direction;         // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll)
     StepType step;                       // see StepType for what steps are performed
     TuneType tune_type;                  // see TuneType
@@ -208,7 +271,7 @@ protected:
     bool     twitch_first_iter;          // true on first iteration of a twitch (used to signal we must step the attitude or rate target)
     bool     use_poshold;                // true = enable position hold
     bool     have_position;              // true = start_position is value
-    Vector3f start_position;
+    Vector3f start_position;             // target when holding position as an offset from EKF origin in cm in NEU frame
     uint8_t  axes_completed;             // bitmask of completed axes
 
     // variables
@@ -256,6 +319,7 @@ protected:
         float current;
     } level_problem;
 
+    // parameters
     AP_Int8  axis_bitmask;
     AP_Float aggressiveness;
     AP_Float min_d;
@@ -267,16 +331,6 @@ protected:
     AP_InertialNav *inertial_nav;
     AP_Motors *motors;
 
-    TuneType  tune_seq[6];              // holds sequence of tune_types to be performed
-    uint8_t tune_seq_curr;               // current tune sequence step
-    virtual bool allow_zero_rate_p() = 0;
-    virtual float get_intra_test_ri(AxisType test_axis) = 0;
-    virtual float get_load_tuned_ri(AxisType test_axis) = 0;
-    virtual float get_load_tuned_yaw_rd() = 0;
-    virtual float get_rp_min() const = 0;
-    virtual float get_sp_min() const = 0;
-    virtual float get_rlpf_min() const = 0;
-
     // Functions added for heli autotune
 
     // Add additional updating gain functions specific to heli
@@ -331,5 +385,4 @@ protected:
 
     max_gain_data max_rate_p;
     max_gain_data max_rate_d;
-
 };

libraries/AC_AutoTune/AC_AutoTune_Heli.cpp

@@ -17,6 +17,8 @@
   Converted to a library by Andrew Tridgell, and rewritten to include helicopters by Bill Geyer
  */
 
+#include "AC_AutoTune_Heli.h"
+
 #define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD     2000   // target roll/pitch angle during AUTOTUNE FeedForward rate test
 #define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS     5000   // target roll/pitch rate during AUTOTUNE FeedForward rate test
 #define AUTOTUNE_FFI_RATIO_FOR_TESTING     0.5f     // I is set 2x smaller than VFF during testing
@@ -36,8 +38,6 @@
 #define AUTOTUNE_SP_MIN                    0.5f     // maximum Stab P value
 #define AUTOTUNE_D_UP_DOWN_MARGIN          0.2f     // The margin below the target that we tune D in
 
-#include "AC_AutoTune_Heli.h"
-
 void AC_AutoTune_Heli::test_init()
 {
 
@@ -262,7 +262,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
     reset();
 }
 
-// generic method used to update gains for the rate p up tune type
+// update gains for the rate p up tune type
 void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
 {
     float p_gain = 0.0f;
@@ -294,7 +294,7 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the rate d up tune type
+// update gains for the rate d up tune type
 void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
 {
     float d_gain = 0.0f;
@@ -326,7 +326,7 @@ void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the rate ff up tune type
+// update gains for the rate ff up tune type
 void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
 {
     switch (test_axis) {
@@ -342,7 +342,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the angle p up tune type
+// update gains for the angle p up tune type
 void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
 {
     // announce results of dwell and update
@@ -361,7 +361,7 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the max gain tune type
+// update gains for the max gain tune type
 void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
 {
 
@@ -675,6 +675,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate
         rate_rads*57.3f);
 }
 
+// get intra test rate I gain for the specified axis
 float AC_AutoTune_Heli::get_intra_test_ri(AxisType test_axis)
 {
     float ret = 0.0f;
@@ -692,7 +693,8 @@ float AC_AutoTune_Heli::get_intra_test_ri(AxisType test_axis)
     return ret;
 }
 
-float AC_AutoTune_Heli::get_load_tuned_ri(AxisType test_axis)
+// get tuned rate I gain for the specified axis
+float AC_AutoTune_Heli::get_tuned_ri(AxisType test_axis)
 {
     float ret = 0.0f;
     switch (test_axis) {
@@ -709,16 +711,21 @@ float AC_AutoTune_Heli::get_load_tuned_ri(AxisType test_axis)
     return ret;
 }
 
+// get minimum rate P (for any axis)
 float AC_AutoTune_Heli::get_rp_min() const
 {
     return AUTOTUNE_RP_MIN;
 }
+
+// get minimum angle P (for any axis)
 float AC_AutoTune_Heli::get_sp_min() const
 {
     return AUTOTUNE_SP_MIN;
 }
 
-float AC_AutoTune_Heli::get_rlpf_min() const
+// get minimum rate Yaw filter value
+float AC_AutoTune_Heli::get_yaw_rate_filt_min() const
 {
     return AUTOTUNE_RLPF_MIN;
 }
+

libraries/AC_AutoTune/AC_AutoTune_Heli.h

@@ -13,8 +13,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall
+  support for autotune of helicopters
-  Converted to a library by Andrew Tridgell
  */
 
 #pragma once
@@ -40,41 +39,67 @@ public:
     void save_tuning_gains() override;
 
 protected:
-    void test_init() override;
+
-    void test_run(AxisType test_axis, const float dir_sign) override;
-    void do_gcs_announcements() override;
     void load_test_gains() override;
 
-    // generic method used to update gains for the rate p up tune type
+    // get intra test rate I gain for the specified axis
+    float get_intra_test_ri(AxisType test_axis) override;
+
+    // get tuned rate I gain for the specified axis
+    float get_tuned_ri(AxisType test_axis) override;
+
+    // get tuned yaw rate d gain
+    float get_tuned_yaw_rd() override { return tune_yaw_rd; }
+
+    void test_init() override;
+    void test_run(AxisType test_axis, const float dir_sign) override;
+
+    // update gains for the rate p up tune type
     void updating_rate_p_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the rate p down tune type
+
+    // update gains for the rate p down tune type
     void updating_rate_p_down_all(AxisType test_axis) override {};
-    // generic method used to update gains for the rate d up tune type
+
+    // update gains for the rate d up tune type
     void updating_rate_d_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the rate d down tune type
+
+    // update gains for the rate d down tune type
     void updating_rate_d_down_all(AxisType test_axis) override {};
-    // generic method used to update gains for the rate ff up tune type
+
+    // update gains for the rate ff up tune type
     void updating_rate_ff_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the rate ff down tune type
+
+    // update gains for the rate ff down tune type
     void updating_rate_ff_down_all(AxisType test_axis) override {};
-    // generic method used to update gains for the angle p up tune type
+
+    // update gains for the angle p up tune type
     void updating_angle_p_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the angle p down tune type
+
+    // update gains for the angle p down tune type
     void updating_angle_p_down_all(AxisType test_axis) override {};
-    // generic method used to update gains for the max gain tune type
+
+    // update gains for the max gain tune type
     void updating_max_gains_all(AxisType test_axis) override;
 
+    // get minimum rate P (for any axis)
+    float get_rp_min() const override;
+
+    // get minimum angle P (for any axis)
+    float get_sp_min() const override;
+
+    // get minimum rate Yaw filter value
+    float get_yaw_rate_filt_min() const override;
+
     void Log_AutoTune() override;
     void Log_AutoTuneDetails() override;
     void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp);
     void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads);
-    bool allow_zero_rate_p() override {return true;}
+
-    float get_intra_test_ri(AxisType test_axis) override;
+    // returns true if rate P gain of zero is acceptable for this vehicle
-    float get_load_tuned_ri(AxisType test_axis) override;
+    bool allow_zero_rate_p() override { return true; }
-    float get_load_tuned_yaw_rd() override {return tune_yaw_rd;}
+
-    float get_rp_min() const override;
+    // send intermittant updates to user on status of tune
-    float get_sp_min() const override;
+    void do_gcs_announcements() override;
-    float get_rlpf_min() const override;
 
 private:
     // updating_rate_ff_up - adjust FF to ensure the target is reached

libraries/AC_AutoTune/AC_AutoTune_Multi.cpp

@@ -36,6 +36,17 @@
 
 #include "AC_AutoTune_Multi.h"
 
+// constructor
+AC_AutoTune_Multi::AC_AutoTune_Multi()
+{
+    tune_seq[0] = RD_UP;
+    tune_seq[1] = RD_DOWN;
+    tune_seq[2] = RP_UP;
+    tune_seq[3] = SP_DOWN;
+    tune_seq[4] = SP_UP;
+    tune_seq[5] = TUNE_COMPLETE;
+}
+
 void AC_AutoTune_Multi::do_gcs_announcements()
 {
     const uint32_t now = AP_HAL::millis();
@@ -199,7 +210,7 @@ void AC_AutoTune_Multi::save_tuning_gains()
     reset();
 }
 
-// generic method used to update gains for the rate p up tune type
+// update gains for the rate p up tune type
 void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
 {
     switch (test_axis) {
@@ -215,7 +226,7 @@ void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the rate d up tune type
+// update gains for the rate d up tune type
 void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
 {
     switch (test_axis) {
@@ -231,7 +242,7 @@ void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the rate d down tune type
+// update gains for the rate d down tune type
 void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
 {
     switch (test_axis) {
@@ -247,7 +258,7 @@ void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the angle p up tune type
+// update gains for the angle p up tune type
 void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
 {
     switch (test_axis) {
@@ -263,7 +274,7 @@ void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
     }
 }
 
-// generic method used to update gains for the angle p down tune type
+// update gains for the angle p down tune type
 void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis)
 {
     switch (test_axis) {
@@ -587,6 +598,7 @@ void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds
         rate_cds*0.01f);
 }
 
+// get intra test rate I gain for the specified axis
 float AC_AutoTune_Multi::get_intra_test_ri(AxisType test_axis)
 {
     float ret = 0.0f;
@@ -604,7 +616,8 @@ float AC_AutoTune_Multi::get_intra_test_ri(AxisType test_axis)
     return ret;
 }
 
-float AC_AutoTune_Multi::get_load_tuned_ri(AxisType test_axis)
+// get tuned rate I gain for the specified axis
+float AC_AutoTune_Multi::get_tuned_ri(AxisType test_axis)
 {
     float ret = 0.0f;
     switch (test_axis) {
@@ -621,17 +634,20 @@ float AC_AutoTune_Multi::get_load_tuned_ri(AxisType test_axis)
     return ret;
 }
 
+// get minimum rate P (for any axis)
 float AC_AutoTune_Multi::get_rp_min() const
 {
     return AUTOTUNE_RP_MIN;
 }
 
+// get minimum angle P (for any axis)
 float AC_AutoTune_Multi::get_sp_min() const
 {
     return AUTOTUNE_SP_MIN;
 }
 
-float AC_AutoTune_Multi::get_rlpf_min() const
+// get minimum rate Yaw filter value
+float AC_AutoTune_Multi::get_yaw_rate_filt_min() const
 {
     return AUTOTUNE_RLPF_MIN;
 }

libraries/AC_AutoTune/AC_AutoTune_Multi.h

@@ -25,69 +25,90 @@ class AC_AutoTune_Multi : public AC_AutoTune
 {
 public:
     // constructor
-    AC_AutoTune_Multi()
+    AC_AutoTune_Multi();
-    {
+
-        tune_seq[0] = RD_UP;
-        tune_seq[1] = RD_DOWN;
-        tune_seq[2] = RP_UP;
-        tune_seq[3] = SP_DOWN;
-        tune_seq[4] = SP_UP;
-        tune_seq[5] = TUNE_COMPLETE;
-    };
     // save gained, called on disarm
     void save_tuning_gains() override;
 
 protected:
+    // load gains
+    void load_test_gains() override;
+
+    // get intra test rate I gain for the specified axis
+    float get_intra_test_ri(AxisType test_axis) override;
+
+    // get tuned rate I gain for the specified axis
+    float get_tuned_ri(AxisType test_axis) override;
+
+    // get tuned yaw rate D gain
+    float get_tuned_yaw_rd() override { return 0.0f; }
+
     void test_init() override;
     void test_run(AxisType test_axis, const float dir_sign) override;
     void do_gcs_announcements() override;
-    void load_test_gains() override;
+
-    // generic method used to update gains for the rate p up tune type
+    // update gains for the rate P up tune type
     void updating_rate_p_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the rate p down tune type
+
+    // update gains for the rate P down tune type
     void updating_rate_p_down_all(AxisType test_axis) override {};
-    // generic method used to update gains for the rate d up tune type
+
+    // update gains for the rate D up tune type
     void updating_rate_d_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the rate d down tune type
+
+    // update gains for the rate D down tune type
     void updating_rate_d_down_all(AxisType test_axis) override;
-    // generic method used to update gains for the rate ff up tune type
+
+    // update gains for the rate ff up tune type
     void updating_rate_ff_up_all(AxisType test_axis) override {};
-    // generic method used to update gains for the rate ff down tune type
+
+    // update gains for the rate ff down tune type
     void updating_rate_ff_down_all(AxisType test_axis) override {};
-    // generic method used to update gains for the angle p up tune type
+
+    // update gains for the angle P up tune type
     void updating_angle_p_up_all(AxisType test_axis) override;
-    // generic method used to update gains for the angle p down tune type
+
+    // update gains for the angle P down tune type
     void updating_angle_p_down_all(AxisType test_axis) override;
-    // generic method used to update gains for the max gain tune type
+
+    // update gains for the max gain tune type
     void updating_max_gains_all(AxisType test_axis) override {};
 
+    // returns true if rate P gain of zero is acceptable for this vehicle
+    bool allow_zero_rate_p() override { return false; }
+
+    // get minimum rate P (for any axis)
+    float get_rp_min() const override;
+
+    // get minimum angle P (for any axis)
+    float get_sp_min() const override;
+
+    // get minimum yaw rate filter value
+    float get_yaw_rate_filt_min() const override;
+
     void Log_AutoTune() override;
     void Log_AutoTuneDetails() override;
     void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
     void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
-    bool allow_zero_rate_p() override {return false;}
-    float get_intra_test_ri(AxisType test_axis) override;
-    float get_load_tuned_ri(AxisType test_axis) override;
-    float get_load_tuned_yaw_rd() override {return 0.0f;}
-    float get_rp_min() const override;
-    float get_sp_min() const override;
-    float get_rlpf_min() const override;
 
 private:
     // updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back
     // optimize D term while keeping the maximum just below the target by adjusting P
     void updating_rate_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 rate_target, float meas_rate_min, float meas_rate_max);
+
     // updating_rate_d_down - decrease D and adjust P to optimize the D term for no bounce back
     // optimize D term while keeping the maximum just below the target by adjusting P
     void updating_rate_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 rate_target, float meas_rate_min, float meas_rate_max);
+
     // updating_rate_p_up_d_down - increase P to ensure the target is reached while checking bounce back isn't increasing
     // P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold
     void updating_rate_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 rate_target, float meas_rate_min, float meas_rate_max);
+
     // updating_angle_p_down - decrease P until we don't reach the target before time out
     // P is decreased to ensure we are not overshooting the target
     void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
+
     // updating_angle_p_up - increase P to ensure the target is reached
     // P is increased until we achieve our target within a reasonable time
     void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
-
 };