Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: PosController alt limit from fence and EKF

This commit is contained in:
Randy Mackay 2015-04-23 14:57:49 +09:00
parent ba94a993f5
commit 37e8d1a338
2 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.pde
View File

@ -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

24
ArduCopter/Attitude.pde
View File

@ -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);
}