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

Plane: new param Q_VFWD_ALT to disable VFWD motor below this altitude. Also uses rangefinder

Browse Source
This commit is contained in:
Tom Pittenger 2016-06-03 15:35:09 -07:00 committed by Andrew Tridgell
parent 021aebeac0
commit 69f29b51d1
2 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/quadplane.cpp
View File

@ -320,6 +320,14 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Standard
AP_GROUPINFO("ESC_CAL", 42, QuadPlane, esc_calibration, 0),
// @Param: VFWD_ALT
// @DisplayName: Forward velocity alt cutoff
// @Description: Controls altitude to disable forward velocity assist when below this relative altitude. This is useful to keep the forward velocity propeller from hitting the ground. Rangefinder height data is incorporated when available.
// @Range: 0 10
// @Increment: 0.25
// @User: Standard
AP_GROUPINFO("VFWD_ALT", 43, QuadPlane, vel_forward_alt_cutoff, 0),
AP_GROUPEND
};
@ -1690,6 +1698,7 @@ int8_t QuadPlane::forward_throttle_pct(void)
if (!plane.ahrs.get_velocity_NED(vel_ned)) {
// we don't know our velocity? EKF must be pretty sick
vel_forward.last_pct = 0;
vel_forward.integrator = 0;
return 0;
}
Vector3f vel_error_body = ahrs.get_rotation_body_to_ned().transposed() * ((desired_velocity_cms*0.01f) - vel_ned);
@ -1717,7 +1726,15 @@ int8_t QuadPlane::forward_throttle_pct(void)
// constrain to throttle range. This allows for reverse throttle if configured
vel_forward.integrator = constrain_float(vel_forward.integrator, MIN(0,plane.aparm.throttle_min), plane.aparm.throttle_max);
vel_forward.last_pct = vel_forward.integrator;
// If we are below alt_cutoff then scale down the effect until it turns off at alt_cutoff and decay the integrator
float alt_cutoff = MAX(0,vel_forward_alt_cutoff);
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator,
height_above_ground, alt_cutoff, alt_cutoff+2);
if (vel_forward.last_pct == 0) {
// if the percent is 0 then decay the integrator
vel_forward.integrator *= 0.95f;
}
return vel_forward.last_pct;
}

1
ArduPlane/quadplane.h
View File

@ -210,6 +210,7 @@ private:
// alt to switch to QLAND_FINAL
AP_Float land_final_alt;
AP_Float vel_forward_alt_cutoff;
AP_Int8 enable;
AP_Int8 transition_pitch_max;