Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: fix guided heading control anti windup

This commit is contained in:
Iampete1 2023-09-23 18:56:42 +01:00 committed by Andrew Tridgell
parent 9e9aaed1b5
commit e9fbf6db24
2 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Plane.h
View File

@ -556,8 +556,7 @@ private:
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;

13
ArduPlane/mode_guided.cpp
View File

@ -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();