Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: limit pitch in transition to QLOITER and QLAND

Browse Source 
when switching to QLOITER or QLAND when in forward flight, limit the
maximum pitch to prevent a sudden decelleration
This commit is contained in:
Andrew Tridgell 2017-09-08 17:54:55 +10:00
parent 2e5c36361e
commit 428233c8b4
2 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduPlane/quadplane.cpp
View File

@ -844,6 +844,9 @@ void QuadPlane::init_loiter(void)
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
init_throttle_wait();
// remember initial pitch
loiter_initial_pitch_cd = MAX(plane.ahrs.pitch_sensor, 0);
}
void QuadPlane::init_land(void)
@ -964,16 +967,29 @@ void QuadPlane::control_loiter()
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller with conservative smoothing gain of 4.0f
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
get_desired_yaw_rate_cds(),
4.0f);
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
uint32_t now = AP_HAL::millis();
if (now - last_pidz_init_ms < (uint32_t)transition_time_ms && !is_tailsitter()) {
// we limit pitch during initial transition
float pitch_limit_cd = linear_interpolate(loiter_initial_pitch_cd, aparm.angle_max,
now,
last_pidz_init_ms, last_pidz_init_ms+transition_time_ms);
if (plane.nav_pitch_cd > pitch_limit_cd) {
plane.nav_pitch_cd = pitch_limit_cd;
pos_control->set_limit_accel_xy();
}
}
// call attitude controller with conservative smoothing gain of 4.0f
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_desired_yaw_rate_cds(),
4.0f);
if (plane.control_mode == QLAND) {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (height_above_ground < land_final_alt && poscontrol.state < QPOS_LAND_FINAL) {

3
ArduPlane/quadplane.h
View File

@ -276,6 +276,9 @@ private:
// last throttle value when active
float last_throttle;
// pitch when we enter loiter mode
int32_t loiter_initial_pitch_cd;
const float smoothing_gain = 6;
// true if we have reached the airspeed threshold for transition