Plane: implement tuning set switching with RC channel

ArduCustom · Dec 26, 2023 · a86c03b · a86c03b
1 parent 64159fd
commit a86c03b
Show file tree

Hide file tree

Showing 6 changed files with 58 additions and 10 deletions.
diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
@@ -186,6 +186,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
     case AUX_FUNC::SERVOS_AUTO_TRIM:
     case AUX_FUNC::EMERGENCY_LANDING_EN:
     case AUX_FUNC::FW_AUTOTUNE:
+    case AUX_FUNC::TUNE_PARAM_SELECT:
         break;
 
     case AUX_FUNC::SOARING:
@@ -435,6 +436,21 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         do_aux_function_armdisarm(ch_flag);
         break;
 
+    case AUX_FUNC::TUNE_PARAM_SELECT: {
+        switch (ch_flag) {
+            case AuxSwitchPos::LOW:
+                plane.tuning.set_current_parmset(plane.tuning.get_parmset1());
+                break;
+            case AuxSwitchPos::MIDDLE:
+                plane.tuning.set_current_parmset(plane.tuning.get_parmset2());
+                break;
+            case AuxSwitchPos::HIGH:
+                plane.tuning.set_current_parmset(plane.tuning.get_parmset3());
+                break;
+        }
+        break;
+    }
+
     default:
         return RC_Channel::do_aux_function(ch_option, ch_flag);
     }

diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
@@ -149,6 +149,8 @@ void Plane::init_ardupilot()
 #if AP_GRIPPER_ENABLED
     g2.gripper.init();
 #endif
+
+    tuning.init();
 }
 
 //********************************************************************************

diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp
@@ -10,7 +10,21 @@ const AP_Param::GroupInfo AP_Tuning_Plane::var_info[] = {
     // @Description: This sets which parameter or set of parameters will be tuned. Values greater than 100 indicate a set of parameters rather than a single parameter. Parameters less than 50 are for QuadPlane vertical lift motors only.
     // @Values: 0:None,1:RateRollPI,2:RateRollP,3:RateRollI,4:RateRollD,5:RatePitchPI,6:RatePitchP,7:RatePitchI,8:RatePitchD,9:RateYawPI,10:RateYawP,11:RateYawI,12:RateYawD,13:AngleRollP,14:AnglePitchP,15:AngleYawP,16:PosXYP,17:PosZP,18:VelXYP,19:VelXYI,20:VelZP,21:AccelZP,22:AccelZI,23:AccelZD,24:RatePitchFF,25:RateRollFF,26:RateYawFF,50:FixedWingRollP,51:FixedWingRollI,52:FixedWingRollD,53:FixedWingRollFF,54:FixedWingPitchP,55:FixedWingPitchI,56:FixedWingPitchD,57:FixedWingPitchFF,58:TRIM_THROTTLE,59:TRIM_PITCH,60:KFF_THRAT2PTCH,61:FBWA max pitch down comp,62:FBWA max pitch down comp thr,63:FWBA pitch down comp curve,64:FBWA max pitch up comp,65:FBWA max pitch up comp thr,66:FWBA pitch up comp curve,67:RLL2PTCH,68:KFF_RDDRMIX,69:TECSTFFDAMP,70:TECSTFF_FILT,71:FWAglRollP,72:FWAglRollI,73:AglRollD,74:AglRollFLTT,75:FWAglPitchP,76:FWAglPitchI,77:AglPitchD,78:AglPitchFLTT,79:MixingDiff,80:MixingOffset,81:THR EXPO MANUAL,82:THR EXPO AUTO,83:FLAP_RETED_SPD,84:FLAP_EXTED_SPD,85:FLAP_EXTED_PCT,86:KFF_THRAT2ELEV,87:MIX_THRAT2ELEVCV,88:KFF_FLAP2ELEV,89:MIX_FLAP2ELEVCV,90:Ailerons diff,91:Elevator diff,92:Q pitch trim,93:THR_AUTO_SRATE,101:Set_RateRollPitch (RATE_ROLL_D / RATE_ROLL_PI / RATE_PITCH_D / RATE_PITCH_PI),102:Set_RateRoll (RATE_ROLL_D / RATE_ROLL_PI),103:Set_RatePitch(RATE_PITCH_D / RATE_PITCH_PI),104:Set_RateYaw (RATE_YAW_P / RATE_YAW_I / RATE_YAW_D),105:Set_AngleRollPitch (ANG_ROLL_P / ANG_PITCH_P),106:Set_VelXY (VXY_P / VXY_I),107:Set_AccelZ (AZ_P / AZ_I / AZ_D),108:Set_RatePitchDP (RATE_PITCH_D / RATE_PITCH_P),109:Set_RateRollDP (RATE_ROLL_D / RATE_ROLL_P),110:Set_RateYawDP (RATE_YAW_D / RATE_YAW_P),111:Set_THR2PTCH (TRIM_THROTTLE / TRIM_PITCH / KFF_THRAT2PTCH / FBWA_PITCH_DOWN / FBWA_MXPTCHD_THR / FBWA_PTCHDN_CRV),112:Set_THR2PTCH_full (TRIM_THROTTLE / TRIM_PITCH / KFF_THRAT2PTCH / FBWA_PITCH_DOWN / FBWA_MXPTCHD_THR / FBWA_PTCHDN_CRV / FBWA_PITCH_UP / FBWA_MXPTCHU_THR / FBWA_PTCHUP_CRV),113:Set turn coordination (RLL2PTCH / KFF_RDDRMIX),114:TECS THR FF (TECS_THR_FF_DAMP / TECS_THR_FF_FILT),115:Set_AglRollPitch (AGL_ROLL_P / AGL_ROLL_D / AGL_ROLL_FLTT / AGL_PITCH_P / AGL_PITCH_D / AGL_PITCH_FLTT),116:Set_AglPitch (AGL_PITCH_D / AGL_PITCH_P / AGL_PITCH_FLTT),117:Set_AglRoll (AGL_ROLL_D / AGL_ROLL_P / AGL_ROLL_FLTT),118:Set_Mixing (MIXING_DIFF / AILERONS_DIFF / ELEVATOR_DIFF / MIXING_OFFSET / MIX_THRAT2ELEV / MIX_THRAT2ELEVCV / MIX_FLAP2ELEV / MIX_FLAP2ELEVCV),119:Set_THR (THR_EXPO_MANUAL / THR_EXPO_AUTO / THR_AUTO_SRATE),120:Set_flap (FLAP_RETED_SPD / FLAP_EXTED_SPD / FLAP_EXTED_PCT),121:Set_FW_Roll_Pitch (RATE_ROLL_D / RATE_ROLL_P / RATE_ROLL_I / RATE_PITCH_D / RATE_PITCH_P / RATE_PITCH_I),122:Set_FW_Pitch (RATE_PITCH_D / RATE_PITCH_P / RATE_PITCH_I),123:Set_FW_Roll (RATE_ROLL_D / RATE_ROLL_P / RATE_ROLL_I),124:Set_FW_Pitch_Roll_PIDS (TUNING_PIT_D / TUNING_PIT_P / TUNING_PIT_I / TUNING_AGL_PITCH_P / TUNING_AGL_PITCH_D / TUNING_AGL_PITCH_FLTT / TUNING_RLL_D / TUNING_RLL_P / TUNING_RLL_I / TUNING_AGL_ROLL_P / TUNING_AGL_ROLL_D / TUNING_AGL_ROLL_FLTT)
     // @User: Standard
-    AP_GROUPINFO("PARAM", 1, AP_Tuning_Plane, parmset, 0),
+    AP_GROUPINFO("PARAM", 1, AP_Tuning_Plane, parmset, 111),
+
+    // @Param: PARAM2
+    // @DisplayName: Transmitter tuning parameter or set of parameters
+    // @Description: This sets which parameter or set of parameters will be tuned. Values greater than 100 indicate a set of parameters rather than a single parameter. Parameters less than 50 are for QuadPlane vertical lift motors only.
+    // @Values: 0:None,1:RateRollPI,2:RateRollP,3:RateRollI,4:RateRollD,5:RatePitchPI,6:RatePitchP,7:RatePitchI,8:RatePitchD,9:RateYawPI,10:RateYawP,11:RateYawI,12:RateYawD,13:AngleRollP,14:AnglePitchP,15:AngleYawP,16:PosXYP,17:PosZP,18:VelXYP,19:VelXYI,20:VelZP,21:AccelZP,22:AccelZI,23:AccelZD,24:RatePitchFF,25:RateRollFF,26:RateYawFF,50:FixedWingRollP,51:FixedWingRollI,52:FixedWingRollD,53:FixedWingRollFF,54:FixedWingPitchP,55:FixedWingPitchI,56:FixedWingPitchD,57:FixedWingPitchFF,58:TRIM_THROTTLE,59:TRIM_PITCH,60:KFF_THRAT2PTCH,61:FBWA max pitch down comp,62:FBWA max pitch down comp thr,63:FWBA pitch down comp curve,64:FBWA max pitch up comp,65:FBWA max pitch up comp thr,66:FWBA pitch up comp curve,67:RLL2PTCH,68:KFF_RDDRMIX,69:TECSTFFDAMP,70:TECSTFF_FILT,71:FWAglRollP,72:FWAglRollI,73:AglRollD,74:AglRollFLTT,75:FWAglPitchP,76:FWAglPitchI,77:AglPitchD,78:AglPitchFLTT,79:MixingDiff,80:MixingOffset,81:THR EXPO MANUAL,82:THR EXPO AUTO,83:FLAP_RETED_SPD,84:FLAP_EXTED_SPD,85:FLAP_EXTED_PCT,86:KFF_THRAT2ELEV,87:MIX_THRAT2ELEVCV,88:KFF_FLAP2ELEV,89:MIX_FLAP2ELEVCV,90:Ailerons diff,91:Elevator diff,92:Q pitch trim,93:THR_AUTO_SRATE,101:Set_RateRollPitch (RATE_ROLL_D / RATE_ROLL_PI / RATE_PITCH_D / RATE_PITCH_PI),102:Set_RateRoll (RATE_ROLL_D / RATE_ROLL_PI),103:Set_RatePitch(RATE_PITCH_D / RATE_PITCH_PI),104:Set_RateYaw (RATE_YAW_P / RATE_YAW_I / RATE_YAW_D),105:Set_AngleRollPitch (ANG_ROLL_P / ANG_PITCH_P),106:Set_VelXY (VXY_P / VXY_I),107:Set_AccelZ (AZ_P / AZ_I / AZ_D),108:Set_RatePitchDP (RATE_PITCH_D / RATE_PITCH_P),109:Set_RateRollDP (RATE_ROLL_D / RATE_ROLL_P),110:Set_RateYawDP (RATE_YAW_D / RATE_YAW_P),111:Set_THR2PTCH (TRIM_THROTTLE / TRIM_PITCH / KFF_THRAT2PTCH / FBWA_PITCH_DOWN / FBWA_MXPTCHD_THR / FBWA_PTCHDN_CRV),112:Set_THR2PTCH_full (TRIM_THROTTLE / TRIM_PITCH / KFF_THRAT2PTCH / FBWA_PITCH_DOWN / FBWA_MXPTCHD_THR / FBWA_PTCHDN_CRV / FBWA_PITCH_UP / FBWA_MXPTCHU_THR / FBWA_PTCHUP_CRV),113:Set turn coordination (RLL2PTCH / KFF_RDDRMIX),114:TECS THR FF (TECS_THR_FF_DAMP / TECS_THR_FF_FILT),115:Set_AglRollPitch (AGL_ROLL_P / AGL_ROLL_D / AGL_ROLL_FLTT / AGL_PITCH_P / AGL_PITCH_D / AGL_PITCH_FLTT),116:Set_AglPitch (AGL_PITCH_D / AGL_PITCH_P / AGL_PITCH_FLTT),117:Set_AglRoll (AGL_ROLL_D / AGL_ROLL_P / AGL_ROLL_FLTT),118:Set_Mixing (MIXING_DIFF / AILERONS_DIFF / ELEVATOR_DIFF / MIXING_OFFSET / MIX_THRAT2ELEV / MIX_THRAT2ELEVCV / MIX_FLAP2ELEV / MIX_FLAP2ELEVCV),119:Set_THR (THR_EXPO_MANUAL / THR_EXPO_AUTO / THR_AUTO_SRATE),120:Set_flap (FLAP_RETED_SPD / FLAP_EXTED_SPD / FLAP_EXTED_PCT),121:Set_FW_Roll_Pitch (RATE_ROLL_D / RATE_ROLL_P / RATE_ROLL_I / RATE_PITCH_D / RATE_PITCH_P / RATE_PITCH_I),122:Set_FW_Pitch (RATE_PITCH_D / RATE_PITCH_P / RATE_PITCH_I),123:Set_FW_Roll (RATE_ROLL_D / RATE_ROLL_P / RATE_ROLL_I),124:Set_FW_Pitch_Roll_PIDS (TUNING_PIT_D / TUNING_PIT_P / TUNING_PIT_I / TUNING_AGL_PITCH_P / TUNING_AGL_PITCH_D / TUNING_AGL_PITCH_FLTT / TUNING_RLL_D / TUNING_RLL_P / TUNING_RLL_I / TUNING_AGL_ROLL_P / TUNING_AGL_ROLL_D / TUNING_AGL_ROLL_FLTT)
+    // @User: Standard
+    AP_GROUPINFO("PARAM2", 2, AP_Tuning_Plane, parmset2, 124),
+
+    // @Param: PARAM
+    // @DisplayName: Transmitter tuning parameter or set of parameters
+    // @Description: This sets which parameter or set of parameters will be tuned. Values greater than 100 indicate a set of parameters rather than a single parameter. Parameters less than 50 are for QuadPlane vertical lift motors only.
+    // @Values: 0:None,1:RateRollPI,2:RateRollP,3:RateRollI,4:RateRollD,5:RatePitchPI,6:RatePitchP,7:RatePitchI,8:RatePitchD,9:RateYawPI,10:RateYawP,11:RateYawI,12:RateYawD,13:AngleRollP,14:AnglePitchP,15:AngleYawP,16:PosXYP,17:PosZP,18:VelXYP,19:VelXYI,20:VelZP,21:AccelZP,22:AccelZI,23:AccelZD,24:RatePitchFF,25:RateRollFF,26:RateYawFF,50:FixedWingRollP,51:FixedWingRollI,52:FixedWingRollD,53:FixedWingRollFF,54:FixedWingPitchP,55:FixedWingPitchI,56:FixedWingPitchD,57:FixedWingPitchFF,58:TRIM_THROTTLE,59:TRIM_PITCH,60:KFF_THRAT2PTCH,61:FBWA max pitch down comp,62:FBWA max pitch down comp thr,63:FWBA pitch down comp curve,64:FBWA max pitch up comp,65:FBWA max pitch up comp thr,66:FWBA pitch up comp curve,67:RLL2PTCH,68:KFF_RDDRMIX,69:TECSTFFDAMP,70:TECSTFF_FILT,71:FWAglRollP,72:FWAglRollI,73:AglRollD,74:AglRollFLTT,75:FWAglPitchP,76:FWAglPitchI,77:AglPitchD,78:AglPitchFLTT,79:MixingDiff,80:MixingOffset,81:THR EXPO MANUAL,82:THR EXPO AUTO,83:FLAP_RETED_SPD,84:FLAP_EXTED_SPD,85:FLAP_EXTED_PCT,86:KFF_THRAT2ELEV,87:MIX_THRAT2ELEVCV,88:KFF_FLAP2ELEV,89:MIX_FLAP2ELEVCV,90:Ailerons diff,91:Elevator diff,92:Q pitch trim,93:THR_AUTO_SRATE,101:Set_RateRollPitch (RATE_ROLL_D / RATE_ROLL_PI / RATE_PITCH_D / RATE_PITCH_PI),102:Set_RateRoll (RATE_ROLL_D / RATE_ROLL_PI),103:Set_RatePitch(RATE_PITCH_D / RATE_PITCH_PI),104:Set_RateYaw (RATE_YAW_P / RATE_YAW_I / RATE_YAW_D),105:Set_AngleRollPitch (ANG_ROLL_P / ANG_PITCH_P),106:Set_VelXY (VXY_P / VXY_I),107:Set_AccelZ (AZ_P / AZ_I / AZ_D),108:Set_RatePitchDP (RATE_PITCH_D / RATE_PITCH_P),109:Set_RateRollDP (RATE_ROLL_D / RATE_ROLL_P),110:Set_RateYawDP (RATE_YAW_D / RATE_YAW_P),111:Set_THR2PTCH (TRIM_THROTTLE / TRIM_PITCH / KFF_THRAT2PTCH / FBWA_PITCH_DOWN / FBWA_MXPTCHD_THR / FBWA_PTCHDN_CRV),112:Set_THR2PTCH_full (TRIM_THROTTLE / TRIM_PITCH / KFF_THRAT2PTCH / FBWA_PITCH_DOWN / FBWA_MXPTCHD_THR / FBWA_PTCHDN_CRV / FBWA_PITCH_UP / FBWA_MXPTCHU_THR / FBWA_PTCHUP_CRV),113:Set turn coordination (RLL2PTCH / KFF_RDDRMIX),114:TECS THR FF (TECS_THR_FF_DAMP / TECS_THR_FF_FILT),115:Set_AglRollPitch (AGL_ROLL_P / AGL_ROLL_D / AGL_ROLL_FLTT / AGL_PITCH_P / AGL_PITCH_D / AGL_PITCH_FLTT),116:Set_AglPitch (AGL_PITCH_D / AGL_PITCH_P / AGL_PITCH_FLTT),117:Set_AglRoll (AGL_ROLL_D / AGL_ROLL_P / AGL_ROLL_FLTT),118:Set_Mixing (MIXING_DIFF / AILERONS_DIFF / ELEVATOR_DIFF / MIXING_OFFSET / MIX_THRAT2ELEV / MIX_THRAT2ELEVCV / MIX_FLAP2ELEV / MIX_FLAP2ELEVCV),119:Set_THR (THR_EXPO_MANUAL / THR_EXPO_AUTO / THR_AUTO_SRATE),120:Set_flap (FLAP_RETED_SPD / FLAP_EXTED_SPD / FLAP_EXTED_PCT),121:Set_FW_Roll_Pitch (RATE_ROLL_D / RATE_ROLL_P / RATE_ROLL_I / RATE_PITCH_D / RATE_PITCH_P / RATE_PITCH_I),122:Set_FW_Pitch (RATE_PITCH_D / RATE_PITCH_P / RATE_PITCH_I),123:Set_FW_Roll (RATE_ROLL_D / RATE_ROLL_P / RATE_ROLL_I),124:Set_FW_Pitch_Roll_PIDS (TUNING_PIT_D / TUNING_PIT_P / TUNING_PIT_I / TUNING_AGL_PITCH_P / TUNING_AGL_PITCH_D / TUNING_AGL_PITCH_FLTT / TUNING_RLL_D / TUNING_RLL_P / TUNING_RLL_I / TUNING_AGL_ROLL_P / TUNING_AGL_ROLL_D / TUNING_AGL_ROLL_FLTT)
+    // @User: Standard
+    AP_GROUPINFO("PARAM3", 3, AP_Tuning_Plane, parmset3, 118),
 
     // the rest of the parameters are from AP_Tuning
     AP_NESTEDGROUPINFO(AP_Tuning, 0),

diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp
@@ -102,7 +102,7 @@ void AP_Tuning::check_selector_switch(void)
                 // re-center the value
                 re_center();
                 gcs().send_text(MAV_SEVERITY_INFO, "Tuning: recentered %s", get_tuning_name(current_parm));
-            } else if (hold_time < 1000) {
+            } else if (hold_time < 3000) {
                 // change parameter
                 next_parameter();
             }
@@ -128,7 +128,7 @@ void AP_Tuning::re_center(void)
  */
 void AP_Tuning::check_input(uint8_t flightmode)
 {
-    if (channel <= 0 || parmset <= 0) {
+    if (channel <= 0 || current_parmset <= 0) {
         // disabled
         return;
     }
@@ -166,10 +166,10 @@ void AP_Tuning::check_input(uint8_t flightmode)
     }
 
     // cope with user changing parmset while tuning
-    if (current_set != parmset) {
+    if (current_set != current_parmset) {
         re_center();
     }
-    current_set = parmset;
+    current_set = current_parmset;
 
     check_selector_switch();
 
@@ -241,7 +241,7 @@ void AP_Tuning::Log_Write_Parameter_Tuning(float value)
 // @Field: CenterValue: Center value (startpoint of current modifications) of parameter being tuned
     AP::logger().Write("PRTN", "TimeUS,Set,Parm,Value,CenterValue", "QBBff",
                                            AP_HAL::micros64(),
-                                           parmset,
+                                           current_parmset,
                                            current_parm,
                                            (double)value,
                                            (double)center_value);
@@ -252,7 +252,7 @@ void AP_Tuning::Log_Write_Parameter_Tuning(float value)
  */
 void AP_Tuning::save_parameters(void)
 {
-    uint8_t set = (uint8_t)parmset.get();
+    uint8_t set = current_parmset;
     if (set < set_base) {
         // single parameter tuning
         save_value(set);
@@ -275,7 +275,7 @@ void AP_Tuning::save_parameters(void)
  */
 void AP_Tuning::revert_parameters(void)
 {
-    uint8_t set = (uint8_t)parmset.get();
+    uint8_t set = current_parmset;
     if (set < set_base) {
         // single parameter tuning
         reload_value(set);
@@ -299,7 +299,7 @@ void AP_Tuning::revert_parameters(void)
  */
 void AP_Tuning::next_parameter(void)
 {
-    uint8_t set = (uint8_t)parmset.get();
+    uint8_t set = current_parmset;
     if (set < set_base) {
         // nothing to do but re-center
         current_parm = set;

diff --git a/libraries/AP_Tuning/AP_Tuning.h b/libraries/AP_Tuning/AP_Tuning.h
@@ -27,7 +27,9 @@ class AP_Tuning
         tuning_names(names) {
         AP_Param::setup_object_defaults(this, var_info);
     }
-
+
+    void init() { set_current_parmset(get_parmset1()); }
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -40,6 +42,14 @@ class AP_Tuning
     const char *get_tuning_name() { return get_tuning_name(current_parm); }
     AP_Float *get_param_pointer() { return get_param_pointer(current_parm); };
 
+    int16_t get_parmset1() { return parmset.get(); }
+    int16_t get_parmset2() { return parmset2.get(); }
+    int16_t get_parmset3() { return parmset3.get(); }
+    void set_current_parmset(int16_t value) {
+        current_parmset = value;
+        next_parameter();
+    }
+
 private:
     AP_Int8 channel;
     AP_Int16 channel_min;
@@ -100,6 +110,10 @@ class AP_Tuning
     virtual void reload_value(uint8_t parm) = 0;
     virtual void set_value(uint8_t parm, float value) = 0;
 
+    int16_t current_parmset = 0;
+
     // parmset is in vehicle subclass var table
     AP_Int16 parmset;
+    AP_Int16 parmset2;
+    AP_Int16 parmset3;
 };
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
@@ -260,6 +260,8 @@ class RC_Channel {
         MOUNT2_PITCH =       216, // mount3 pitch input
         MOUNT2_YAW =         217, // mount4 yaw input
 
+        TUNE_PARAM_SELECT =  250,
+
         // inputs 248-249 are reserved for the Skybrush fork at
         // https://github.com/skybrush-io/ardupilot