From a3812d7632712059b5d0ff9749dcf67b4598234d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 17 Dec 2022 23:25:09 +1030 Subject: [PATCH] Copter: Tuning for position controller angle max --- ArduCopter/Parameters.cpp | 2 +- ArduCopter/defines.h | 3 ++- ArduCopter/tuning.cpp | 4 ++++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a13ae4faac..3ec8cd373d 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -337,7 +337,7 @@ const AP_Param::Info Copter::var_info[] = { // @DisplayName: Channel 6 Tuning // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob // @User: Standard - // @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 Roll/Pitch deg/s,40:Acro Yaw deg/s,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,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,58:SysID Magnitude + // @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 Roll/Pitch deg/s,40:Acro Yaw deg/s,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,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,58:SysID Magnitude,59:PSC Angle Max GSCALAR(radio_tuning, "TUNE", 0), // @Param: FRAME_TYPE diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7fa78252a7..708042713a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -58,7 +58,8 @@ enum tuning_func { TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum TUNING_RATE_YAW_FILT = 56, // yaw rate input filter UNUSED = 57, // was winch control - TUNING_SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal + TUNING_SYSTEM_ID_MAGNITUDE = 58, // magnitude of the system ID signal + TUNING_POS_CONTROL_ANGLE_MAX = 59 // position controller maximum angle }; // Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 0cd125782e..db7f15c889 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -189,5 +189,9 @@ void Copter::tuning() copter.mode_systemid.set_magnitude(tuning_value); #endif break; + + case TUNING_POS_CONTROL_ANGLE_MAX: + pos_control->set_lean_angle_max_cd(tuning_value * 100.0); + break; } }