mirror of https://github.com/ArduPilot/ardupilot
Plane: fix guided heading control anti windup
This commit is contained in:
parent
9e9aaed1b5
commit
e9fbf6db24
|
@ -556,8 +556,7 @@ private:
|
||||||
float target_heading_accel_limit;
|
float target_heading_accel_limit;
|
||||||
uint32_t target_heading_time_ms;
|
uint32_t target_heading_time_ms;
|
||||||
guided_heading_type_t target_heading_type;
|
guided_heading_type_t target_heading_type;
|
||||||
bool target_heading_limit_low;
|
bool target_heading_limit;
|
||||||
bool target_heading_limit_high;
|
|
||||||
#endif // OFFBOARD_GUIDED == ENABLED
|
#endif // OFFBOARD_GUIDED == ENABLED
|
||||||
} guided_state;
|
} guided_state;
|
||||||
|
|
||||||
|
|
|
@ -61,16 +61,11 @@ void ModeGuided::update()
|
||||||
float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
|
float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
|
||||||
bank_limit = MIN(bank_limit, plane.roll_limit_cd);
|
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
|
// Check for output saturation
|
||||||
if (((is_negative(error) && !plane.guided_state.target_heading_limit_low) || (is_positive(error) && !plane.guided_state.target_heading_limit_high))) {
|
plane.guided_state.target_heading_limit = fabsf(desired) >= bank_limit;
|
||||||
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);
|
|
||||||
|
|
||||||
plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
|
plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
|
||||||
plane.update_load_factor();
|
plane.update_load_factor();
|
||||||
|
|
Loading…
Reference in New Issue