Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Alt Hold init bug

Browse Source
This commit is contained in:
Leonard Hall 2019-07-16 11:43:55 +09:30 committed by Randy Mackay
parent deb3d6d8c3
commit 78be1067a8
3 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -215,7 +215,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& ina
_flags.reset_desired_vel_to_pos = true;
_flags.reset_accel_to_lean_xy = true;
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
_flags.freeze_ff_z = true;
_flags.use_desvel_ff_z = true;
_limit.pos_up = true;
@ -378,9 +377,10 @@ void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
_vel_last.z = _inav.get_velocity_z();
_accel_desired.z = 0.0f;
_accel_last_z_cms = 0.0f;
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_flags.reset_accel_to_throttle = true;
_flags.reset_rate_to_accel_z = true;
_pid_accel_z.set_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f);
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();
}
// get_alt_error - returns altitude error in cm
@ -468,7 +468,9 @@ void AC_PosControl::update_z_controller()
const uint64_t now_us = AP_HAL::micros64();
if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
_pid_accel_z.set_integrator((_motors.get_throttle() - _motors.get_throttle_hover()) * 1000.0f);
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();
}
_last_update_z_us = now_us;
@ -588,15 +590,8 @@ void AC_PosControl::run_z_controller()
// Calculate Earth Frame Z acceleration
z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
// reset target acceleration if this controller has just been engaged
if (_flags.reset_accel_to_throttle) {
// Reset Filter
_accel_error.z = 0;
_flags.reset_accel_to_throttle = false;
} else {
// calculate accel error
_accel_error.z = _accel_target.z - z_accel_meas;
}
// calculate accel error
_accel_error.z = _accel_target.z - z_accel_meas;
// set input to PID
_pid_accel_z.set_input_filter_all(_accel_error.z);

1
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -309,7 +309,6 @@ protected:
uint16_t reset_desired_vel_to_pos : 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_z : 1; // 1 used to freeze velocity to accel feed forward for one iteration
uint16_t use_desvel_ff_z : 1; // 1 to use z-axis desired velocity as feed forward into velocity step
uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep

4
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
View File

@ -99,6 +99,8 @@ void AC_PosControl_Sub::relax_alt_hold_controllers()
_vel_last.z = _inav.get_velocity_z();
_accel_desired.z = 0.0f;
_accel_last_z_cms = 0.0f;
_flags.reset_rate_to_accel_z = true;
_pid_accel_z.set_integrator(-_motors.get_throttle_hover() * 1000.0f);
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_flags.reset_accel_to_throttle = true;
_pid_accel_z.reset_filter();
}