aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PosControl: fix twitch when entering RTL

Browse Source 
Also removed slow_cpu flag
Fixed bug in update to _accel_target_jerk_limited.
This commit is contained in:
Randy Mackay 2015-03-13 20:46:21 +09:00
parent 8f6982860f
commit bb74b8dec8
2 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -50,15 +50,11 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
AP_Param::setup_object_defaults(this, var_info);
// initialise flags
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
_flags.slow_cpu = false;
#else
_flags.slow_cpu = true;
#endif
_flags.recalc_leash_xy = true;
_flags.recalc_leash_z = true;
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true;
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
@ -520,6 +516,7 @@ void AC_PosControl::init_xy_controller(bool reset_I)
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true;
}
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
@ -571,6 +568,7 @@ void AC_PosControl::init_vel_controller_xyz()
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true;
// set target position in xy axis
const Vector3f& curr_pos = _inav.get_position();
@ -803,6 +801,15 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
float accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max();
// reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) {
_accel_target_jerk_limited.x = _accel_target.x;
_accel_target_jerk_limited.y = _accel_target.y;
_accel_target_filtered.x = _accel_target.x;
_accel_target_filtered.y = _accel_target.y;
_flags.reset_accel_to_lean_xy = false;
}
// apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS;
@ -812,8 +819,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
if(accel_change_length > max_delta_accel) {
accel_change *= max_delta_accel/accel_change_length;
_accel_target_jerk_limited += accel_change;
}
_accel_target_jerk_limited += accel_change;
// 5Hz lowpass filter on NE accel
float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler;

18
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -263,15 +263,15 @@ private:
// general purpose flags
struct poscontrol_flags {
uint8_t recalc_leash_z : 1; // 1 if we should recalculate the z axis leash length
uint8_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length
uint8_t slow_cpu : 1; // 1 if we are running on a slow_cpu machine. xy position control is broken up into multiple steps
uint8_t reset_desired_vel_to_pos: 1; // 1 if we should reset the rate_to_accel_xy step
uint8_t reset_rate_to_accel_xy : 1; // 1 if we should reset the rate_to_accel_xy step
uint8_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step
uint8_t reset_accel_to_throttle : 1; // 1 if we should reset the accel_to_throttle step of the z-axis controller
uint8_t freeze_ff_xy : 1; // 1 use to freeze feed forward during step updates
uint8_t freeze_ff_z : 1; // 1 use to freeze feed forward during step updates
uint16_t recalc_leash_z : 1; // 1 if we should recalculate the z axis leash length
uint16_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length
uint16_t reset_desired_vel_to_pos : 1; // 1 if we should reset the rate_to_accel_xy step
uint16_t reset_rate_to_accel_xy : 1; // 1 if we should reset the rate_to_accel_xy step
uint16_t reset_accel_to_lean_xy : 1; // 1 if we should reset the accel to lean angle step
uint16_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step
uint16_t reset_accel_to_throttle : 1; // 1 if we should reset the accel_to_throttle step of the z-axis controller
uint16_t freeze_ff_xy : 1; // 1 use to freeze feed forward during step updates
uint16_t freeze_ff_z : 1; // 1 use to freeze feed forward during step updates
} _flags;
// limit flags structure