From 8b002204df7e4ea7504b837ff51e9ff54324c805 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 23 Sep 2023 18:56:42 +0100 Subject: [PATCH 1/3] Plane: fix guided heading control anti windup --- ArduPlane/Plane.h | 3 +-- ArduPlane/mode_guided.cpp | 13 ++++--------- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1385e5cbc4d55..9634184c83078 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -556,8 +556,7 @@ class Plane : public AP_Vehicle { float target_heading_accel_limit; uint32_t target_heading_time_ms; guided_heading_type_t target_heading_type; - bool target_heading_limit_low; - bool target_heading_limit_high; + bool target_heading_limit; #endif // OFFBOARD_GUIDED == ENABLED } guided_state; diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 2d712c7598722..a523ddf87065e 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -61,16 +61,11 @@ void ModeGuided::update() float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; bank_limit = MIN(bank_limit, plane.roll_limit_cd); - plane.g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.? + // push error into AC_PID + const float desired = plane.g2.guidedHeading.update_error(error, delta, plane.guided_state.target_heading_limit); - float i = plane.g2.guidedHeading.get_i(); // get integrator TODO - if (((is_negative(error) && !plane.guided_state.target_heading_limit_low) || (is_positive(error) && !plane.guided_state.target_heading_limit_high))) { - i = plane.g2.guidedHeading.get_i(); - } - - float desired = plane.g2.guidedHeading.get_p() + i + plane.g2.guidedHeading.get_d(); - plane.guided_state.target_heading_limit_low = (desired <= -bank_limit); - plane.guided_state.target_heading_limit_high = (desired >= bank_limit); + // Check for output saturation + plane.guided_state.target_heading_limit = fabsf(desired) >= bank_limit; plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit); plane.update_load_factor(); From 33463375f682df92830298a6eec0742bc4a71fee Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 29 Sep 2023 02:52:46 +0100 Subject: [PATCH 2/3] Plane: MAV_CMD_GUIDED_CHANGE_HEADING: allow changing heading type or rate for same heading --- ArduPlane/GCS_Mavlink.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 15593a66fd7b0..7bc9f72ce7cb4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -919,11 +919,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); - // if packet is requesting us to go to the heading we are already going to, we-re already on it. - if ( (is_equal(new_target_heading,plane.guided_state.target_heading))) { // compare two floats as near-enough - return MAV_RESULT_ACCEPTED; - } - // course over ground if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int plane.guided_state.target_heading_type = GUIDED_HEADING_COG; From b14dc27a2e62eb0e42e1d17510d75fb1c07f0564 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 29 Sep 2023 02:53:10 +0100 Subject: [PATCH 3/3] Plane: GuidedHeading PID: remove slew limit --- ArduPlane/Parameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 3045ff8eda7e0..e9c25ea5da174 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -548,7 +548,7 @@ class ParametersG2 { #if OFFBOARD_GUIDED == ENABLED // guided yaw heading PID - AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2}; + AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.0}; #endif #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED