mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-02 19:53:57 -04:00
Copter: remove setting position control's altitude max
AC_Avoid now takes responsibility for enforcing the alt limit and accesses inertial nav's limit directly
This commit is contained in:
parent
62a4867cd4
commit
cb1f7ba4bb
@ -504,9 +504,6 @@ void Copter::one_hz_loop()
|
||||
|
||||
check_usb_mux();
|
||||
|
||||
// update position controller alt limits
|
||||
update_poscon_alt_max();
|
||||
|
||||
// enable/disable raw gyro/accel logging
|
||||
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
||||
|
||||
|
@ -302,17 +302,6 @@ void Copter::set_accel_throttle_I_from_pilot_throttle()
|
||||
g.pid_accel_z.set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
|
||||
}
|
||||
|
||||
// updates position controller's maximum altitude using fence and EKF limits
|
||||
void Copter::update_poscon_alt_max()
|
||||
{
|
||||
// get alt limit from EKF (limited during optical flow flight)
|
||||
float ekf_limit_cm;
|
||||
if (inertial_nav.get_hgt_ctrl_limit(ekf_limit_cm)) {
|
||||
// pass limit to pos controller
|
||||
pos_control->set_alt_max(ekf_limit_cm);
|
||||
}
|
||||
}
|
||||
|
||||
// rotate vector from vehicle's perspective to North-East frame
|
||||
void Copter::rotate_body_frame_to_NE(float &x, float &y)
|
||||
{
|
||||
|
@ -668,7 +668,6 @@ private:
|
||||
void auto_takeoff_set_start_alt(void);
|
||||
void auto_takeoff_attitude_run(float target_yaw_rate);
|
||||
void set_accel_throttle_I_from_pilot_throttle();
|
||||
void update_poscon_alt_max();
|
||||
void rotate_body_frame_to_NE(float &x, float &y);
|
||||
void gcs_send_heartbeat(void);
|
||||
void gcs_send_deferred(void);
|
||||
|
Loading…
Reference in New Issue
Block a user