diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 4fa6df1489..ac3728df83 100644 --- 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 index c4643e9a79..c3c7f4fa6f 100644 --- 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 index 1f3c4dd567..617d4ec9d2 100644 --- 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 index 39e8853254..87d46a4494 100644 --- 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; @@ -322,6 +322,27 @@ void AP_Tuning::next_parameter(void) } } +void AP_Tuning::set_current_parmset(int16_t value) +{ + current_parmset = value; + if (current_parmset < set_base) { + // nothing to do but re-center + current_parm = current_parmset; + re_center(); + return; + } + for (uint8_t i=0; tuning_sets[i].num_parms != 0; i++) { + if (tuning_sets[i].set+set_base == current_parmset) { + current_parm_index = 0; + current_parm = tuning_sets[i].parms[current_parm_index]; + re_center(); + gcs().send_text(MAV_SEVERITY_INFO, "Tuning: started %s", get_tuning_name(current_parm)); + AP_Notify::events.tune_next = current_parm_index+1; + break; + } + } +} + /* return a string representing a tuning parameter */ diff --git a/libraries/AP_Tuning/AP_Tuning.h b/libraries/AP_Tuning/AP_Tuning.h index 1403250ab1..f475eb4ed0 100644 --- 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,11 @@ 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); + private: AP_Int8 channel; AP_Int16 channel_min; @@ -100,6 +107,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 index 44c142578f..c375d9a2b3 100644 --- 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