Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane: fix guided heading control anti windup #25085

Merged
merged 3 commits into from
Oct 3, 2023
Merged

Plane: fix guided heading control anti windup #25085

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8b00220
Plane: fix guided heading control anti windup
IamPete1 Sep 23, 2023
3346337
Plane: MAV_CMD_GUIDED_CHANGE_HEADING: allow changing heading type or …
IamPete1 Sep 29, 2023
b14dc27
Plane: GuidedHeading PID: remove slew limit
IamPete1 Sep 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 0 additions & 5 deletions ArduPlane/GCS_Mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 1 addition & 2 deletions ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
13 changes: 4 additions & 9 deletions ArduPlane/mode_guided.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down