Browse Source

AC_AttitudeControl: Support seperate roll and pitch limits

master
Leonard Hall 6 years ago committed by Randy Mackay
parent
commit
7f5d6662b3
  1. 4
      libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
  2. 4
      libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@ -329,10 +329,10 @@ void AC_AttitudeControl_Multi::rate_controller_run()
Vector3f gyro_latest = _ahrs.get_gyro_latest(); Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll_pitch)); _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll));
_motors.set_roll_ff(get_rate_roll_pid().get_ff()); _motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.roll_pitch)); _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch));
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw)); _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp

@ -335,8 +335,8 @@ void AC_AttitudeControl_Sub::rate_controller_run()
update_throttle_rpy_mix(); update_throttle_rpy_mix();
Vector3f gyro_latest = _ahrs.get_gyro_latest(); Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll_pitch)); _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll));
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.roll_pitch)); _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch));
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw)); _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));
control_monitor_update(); control_monitor_update();

Loading…
Cancel
Save