mirror of https://github.com/ArduPilot/ardupilot
Copter: PosController alt limit from fence and EKF
This commit is contained in:
parent
ba94a993f5
commit
37e8d1a338
|
@ -1082,12 +1082,8 @@ static void one_hz_loop()
|
|||
#endif
|
||||
#endif
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// set fence altitude limit in position controller
|
||||
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
|
||||
pos_control.set_alt_max(pv_alt_above_origin(fence.get_safe_alt()*100.0f));
|
||||
}
|
||||
#endif
|
||||
// update position controller alt limits
|
||||
update_poscon_alt_max();
|
||||
}
|
||||
|
||||
// called at 50hz
|
||||
|
|
|
@ -266,3 +266,27 @@ static void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
|
|||
// shift difference between pilot's throttle and hover throttle into accelerometer I
|
||||
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
|
||||
}
|
||||
|
||||
// updates position controller's maximum altitude using fence and EKF limits
|
||||
static void update_poscon_alt_max()
|
||||
{
|
||||
float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// set fence altitude limit in position controller
|
||||
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
|
||||
alt_limit_cm = pv_alt_above_origin(fence.get_safe_alt()*100.0f);
|
||||
}
|
||||
#endif
|
||||
|
||||
// get alt limit from EKF (limited during optical flow flight)
|
||||
float ekf_limit_cm = 0.0f;
|
||||
if (inertial_nav.get_hgt_ctrl_limit(ekf_limit_cm)) {
|
||||
if ((alt_limit_cm <= 0.0f) || (ekf_limit_cm < alt_limit_cm)) {
|
||||
alt_limit_cm = ekf_limit_cm;
|
||||
}
|
||||
}
|
||||
|
||||
// pass limit to pos controller
|
||||
pos_control.set_alt_max(alt_limit_cm);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue