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

ArduPlane: add thr position trigger to ThrLandCtrl Q_OPTION

Browse Source
This commit is contained in:
Hwurzburg 2021-08-17 10:29:42 -05:00 committed by Peter Barker
parent 784d1ddf7c
commit 551d55a85c
3 changed files with 32 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/config.h
View File

@ -286,3 +286,9 @@
#ifndef FS_EKF_THRESHOLD_DEFAULT
# define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
#endif
/////////////////////////////////////////////////////////////////////////////
// Landing Throttle Control Trigger Threshold
#ifndef THR_CTRL_LAND_THRESH
#define THR_CTRL_LAND_THRESH 0.7
#endif

34
ArduPlane/quadplane.cpp
View File

@ -1223,7 +1223,7 @@ bool QuadPlane::is_flying_vtol(void) const
smooth out descent rate for landing to prevent a jerk as we get to
land_final_alt.
*/
float QuadPlane::landing_descent_rate_cms(float height_above_ground) const
float QuadPlane::landing_descent_rate_cms(float height_above_ground)
{
if (poscontrol.get_state() == QPOS_LAND_FINAL) {
// when in final use descent rate for final even if alt has climbed again
@ -1237,19 +1237,24 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground) const
if ((options & OPTION_THR_LANDING_CONTROL) != 0) {
// allow throttle control for landing speed
const float thr_in = get_pilot_land_throttle();
const float dz = 0.1;
const float thresh1 = 0.5+dz;
const float thresh2 = 0.5-dz;
const float scaling = 1.0 / (0.5 - dz);
if (thr_in > thresh1) {
// start climbing
ret = -(thr_in - thresh1)*scaling*max_climb_speed;
} else if (thr_in > thresh2) {
// hold height
ret = 0;
} else {
ret *= (thresh2 - thr_in)*scaling;
if (thr_in > THR_CTRL_LAND_THRESH) {
thr_ctrl_land = true;
}
if (thr_ctrl_land) {
const float dz = 0.1;
const float thresh1 = 0.5+dz;
const float thresh2 = 0.5-dz;
const float scaling = 1.0 / (0.5 - dz);
if (thr_in > thresh1) {
// start climbing
ret = -(thr_in - thresh1)*scaling*max_climb_speed;
} else if (thr_in > thresh2) {
// hold height
ret = 0;
} else {
ret *= (thresh2 - thr_in)*scaling;
}
}
}
if (poscontrol.pilot_correction_active) {
@ -2302,6 +2307,9 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
} else if (s == QPOS_AIRBRAKE) {
// start with zero integrator on vertical throttle
qp.pos_control->get_accel_z_pid().set_integrator(0);
} else if (s == QPOS_LAND_DESCEND) {
// reset throttle descent control
qp.thr_ctrl_land = false;
}
}
state = s;

7
ArduPlane/quadplane.h
View File

@ -122,6 +122,9 @@ public:
// return true if the user has set ENABLE
bool enabled(void) const { return enable != 0; }
// is throttle controlled landing descent active?
bool thr_ctrl_land;
uint16_t get_pilot_velocity_z_max_dn() const;
@ -239,7 +242,7 @@ private:
bool should_relax(void);
void motors_output(bool run_rate_controller = true);
void Log_Write_QControl_Tuning();
float landing_descent_rate_cms(float height_above_ground) const;
float landing_descent_rate_cms(float height_above_ground);
// setup correct aux channels for frame class
void setup_default_channels(uint8_t num_motors);
@ -396,7 +399,7 @@ private:
// are we in a guided takeoff?
bool guided_takeoff:1;
struct {
// time when motors reached lower limit
uint32_t lower_limit_start_ms;