From a5bdd12bfa917784ee19ee2efd08488a0ad10514 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Apr 2019 21:32:26 +0900 Subject: [PATCH] Copter: replace TUNE_LOW/HIGH params with TUNE_MIN/MAX change from uint16 to floats to ease setup also add check that no function assigned to rc6 --- ArduCopter/Copter.h | 2 +- ArduCopter/Log.cpp | 16 +++++------ ArduCopter/Parameters.cpp | 42 +++++++++++++++++----------- ArduCopter/Parameters.h | 8 +++--- ArduCopter/tuning.cpp | 59 +++++++++++++++++++-------------------- 5 files changed, 66 insertions(+), 61 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c8c7d43e71..0869b8cfd2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -721,7 +721,7 @@ private: void Log_Write_Data(uint8_t id, int16_t value); void Log_Write_Data(uint8_t id, uint16_t value); void Log_Write_Data(uint8_t id, float value); - void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high); + void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); void Log_Sensor_Health(); #if FRAME_CONFIG == HELI_FRAME void Log_Write_Heli(void); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 5288b70c32..37f98dfa43 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -234,21 +234,19 @@ struct PACKED log_ParameterTuning { uint64_t time_us; uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE float tuning_value; // normalized value used inside tuning() function - int16_t control_in; // raw tune input value - int16_t tuning_low; // tuning low end value - int16_t tuning_high; // tuning high end value + float tuning_min; // tuning minimum value + float tuning_max; // tuning maximum value }; -void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) +void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) { struct log_ParameterTuning pkt_tune = { LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG), time_us : AP_HAL::micros64(), parameter : param, tuning_value : tuning_val, - control_in : control_in, - tuning_low : tune_low, - tuning_high : tune_high + tuning_min : tune_min, + tuning_max : tune_max }; logger.WriteBlock(&pkt_tune, sizeof(pkt_tune)); @@ -388,7 +386,7 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar const struct LogStructure Copter::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning), - "PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" }, + "PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), @@ -443,7 +441,7 @@ void Copter::Log_Write_Data(uint8_t id, uint32_t value) {} void Copter::Log_Write_Data(uint8_t id, int16_t value) {} void Copter::Log_Write_Data(uint8_t id, uint16_t value) {} void Copter::Log_Write_Data(uint8_t id, float value) {} -void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {} +void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} void Copter::Log_Sensor_Health() {} void Copter::Log_Write_Precland() {} void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 987b225177..5035514578 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -337,20 +337,6 @@ const AP_Param::Info Copter::var_info[] = { // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch GSCALAR(radio_tuning, "TUNE", 0), - // @Param: TUNE_LOW - // @DisplayName: Tuning minimum - // @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob - // @User: Standard - // @Range: 0 32767 - GSCALAR(radio_tuning_low, "TUNE_LOW", 0), - - // @Param: TUNE_HIGH - // @DisplayName: Tuning maximum - // @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob - // @User: Standard - // @Range: 0 32767 - GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000), - // @Param: FRAME_TYPE // @DisplayName: Frame Type (+, X, V, etc) // @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters. @@ -927,11 +913,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // Scripting is intentionally not showing up in the parameter docs until it is a more standard feature AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), #endif - + + // @Param: TUNE_MIN + // @DisplayName: Tuning minimum + // @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to + // @User: Standard + AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0), + + // @Param: TUNE_MAX + // @DisplayName: Tuning maximum + // @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to + // @User: Standard + AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0), + AP_GROUPEND }; -// These CHx_OPT param descriptions are here so that users of beta Mission Planner (which uses the master branch as its source of descriptions) +// These param descriptions are here so that users of beta Mission Planner (which uses the master branch as its source of descriptions) // can get them. These lines can be removed once Copter-3.7.0-beta testing begins or we improve the source of descriptions for GCSs. // // @Param: CH7_OPT @@ -970,6 +968,18 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard +// @Param: TUNE_LOW +// @DisplayName: Tuning minimum +// @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob +// @User: Standard +// @Range: 0 32767 + +// @Param: TUNE_HIGH +// @DisplayName: Tuning maximum +// @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob +// @User: Standard +// @Range: 0 32767 + /* constructor for g2 object */ diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 2eeac9acc2..bd1d20f8bd 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -285,8 +285,8 @@ public: k_param_throttle_trim, // remove k_param_esc_calibrate, k_param_radio_tuning, - k_param_radio_tuning_high, - k_param_radio_tuning_low, + k_param_radio_tuning_high_old, // unused + k_param_radio_tuning_low_old, // unused k_param_rc_speed = 192, k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor k_param_throttle_mid, // remove @@ -428,8 +428,6 @@ public: AP_Int32 log_bitmask; AP_Int8 esc_calibrate; AP_Int8 radio_tuning; - AP_Int16 radio_tuning_high; - AP_Int16 radio_tuning_low; AP_Int8 frame_type; AP_Int8 disarm_delay; @@ -587,6 +585,8 @@ public: AP_Scripting scripting; #endif // ENABLE_SCRIPTING + AP_Float tuning_min; + AP_Float tuning_max; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 67c90c2e65..bb1c3d06b8 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -7,23 +7,28 @@ // tuning - updates parameters based on the ch6 tuning knob's position // should be called at 3.3hz -void Copter::tuning() { - RC_Channel *rc6 = rc().channel(CH_6); +void Copter::tuning() +{ + const RC_Channel *rc6 = rc().channel(CH_6); - // exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero - if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) { + // exit immediately if the tuning function is not set or min and max are both zero + if ((g.radio_tuning <= 0) || (is_zero(g2.tuning_min.get()) && is_zero(g2.tuning_max.get()))) { return; } - uint16_t radio_in = rc6->get_radio_in(); - float v = constrain_float((radio_in - rc6->get_radio_min()) / float(rc6->get_radio_max() - rc6->get_radio_min()), 0, 1); - int16_t control_in = g.radio_tuning_low + v * (g.radio_tuning_high - g.radio_tuning_low); - float tuning_value = control_in / 1000.0f; - - // Tuning Value should never be outside the bounds of the specified low and high value - tuning_value = constrain_float(tuning_value, g.radio_tuning_low/1000.0f, g.radio_tuning_high/1000.0f); + // exit immediately when radio failsafe is invoked or transmitter has not been turned on + if (failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) { + return; + } - Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, control_in, g.radio_tuning_low, g.radio_tuning_high); + // exit immediately if a function is assigned to channel 6 + if ((RC_Channel::aux_func_t)rc6->option.get() != RC_Channel::AUX_FUNC::DO_NOTHING) { + return; + } + + const uint16_t radio_in = rc6->get_radio_in(); + float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max()); + Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max); switch(g.radio_tuning) { @@ -96,8 +101,7 @@ void Copter::tuning() { break; case TUNING_WP_SPEED: - // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s - wp_nav->set_speed_xy(control_in); + wp_nav->set_speed_xy(tuning_value); break; // Acro roll pitch gain @@ -112,7 +116,7 @@ void Copter::tuning() { #if FRAME_CONFIG == HELI_FRAME case TUNING_HELI_EXTERNAL_GYRO: - motors->ext_gyro_gain((float)control_in / 1000.0f); + motors->ext_gyro_gain(tuning_value); break; case TUNING_RATE_PITCH_FF: @@ -129,14 +133,12 @@ void Copter::tuning() { #endif case TUNING_DECLINATION: - // set declination to +-20degrees - compass.set_declination(ToRad((2.0f * control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact + compass.set_declination(ToRad(tuning_value), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact break; #if MODE_CIRCLE_ENABLED == ENABLED case TUNING_CIRCLE_RATE: - // set circle rate up to approximately 45 deg/sec in either direction - circle_nav->set_rate((float)control_in/25.0f-20.0f); + circle_nav->set_rate(tuning_value); break; #endif @@ -179,8 +181,7 @@ void Copter::tuning() { #endif case TUNING_RC_FEEL_RP: - // convert from control_in to input time constant - attitude_control->set_input_tc(1.0f / (2.0f + MAX((control_in * 0.01f), 0.0f))); + attitude_control->set_input_tc(tuning_value); break; case TUNING_RATE_PITCH_KP: @@ -209,7 +210,7 @@ void Copter::tuning() { #if FRAME_CONFIG != HELI_FRAME case TUNING_RATE_MOT_YAW_HEADROOM: - motors->set_yaw_headroom(tuning_value*1000); + motors->set_yaw_headroom(tuning_value); break; #endif @@ -218,17 +219,13 @@ void Copter::tuning() { break; #if WINCH_ENABLED == ENABLED - case TUNING_WINCH: { - float desired_rate = 0.0f; - if (v > 0.6f) { - desired_rate = g2.winch.get_rate_max() * (v - 0.6f) / 0.4f; + case TUNING_WINCH: + // add small deadzone + if (fabsf(tuning_value) < 0.05f) { + tuning_value = 0; } - if (v < 0.4f) { - desired_rate = g2.winch.get_rate_max() * (v - 0.4) / 0.4f; - } - g2.winch.set_desired_rate(desired_rate); + g2.winch.set_desired_rate(tuning_value); break; - } #endif } }