forked from Archive/PX4-Autopilot
added max pitch parameter for climbout phase
This commit is contained in:
parent
178ec7f4fc
commit
0769ec5345
|
@ -65,6 +65,7 @@ RunwayTakeoff::RunwayTakeoff() :
|
|||
_runway_takeoff_nav_alt(this, "NAV_ALT"),
|
||||
_runway_takeoff_throttle(this, "MAX_THR"),
|
||||
_runway_takeoff_pitch_sp(this, "PSP"),
|
||||
_runway_takeoff_max_pitch(this, "MAX_PITCH"),
|
||||
_airspeed_min(this, "FW_AIRSPD_MIN", false),
|
||||
_climbout_diff(this, "FW_CLMBOUT_DIFF", false)
|
||||
{
|
||||
|
@ -83,6 +84,7 @@ void RunwayTakeoff::init(float yaw)
|
|||
_initialized = true;
|
||||
_state = RunwayTakeoffState::THROTTLE_RAMP;
|
||||
_initialized_time = hrt_absolute_time();
|
||||
_climbout = true;
|
||||
}
|
||||
|
||||
void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
|
||||
|
@ -98,7 +100,6 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
|
|||
|
||||
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
|
||||
if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) {
|
||||
_climbout = true;
|
||||
_state = RunwayTakeoffState::TAKEOFF;
|
||||
mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached");
|
||||
}
|
||||
|
@ -218,6 +219,17 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
|
|||
}
|
||||
}
|
||||
|
||||
float RunwayTakeoff::getMaxPitch(float max)
|
||||
{
|
||||
if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) {
|
||||
return _runway_takeoff_max_pitch.get();
|
||||
}
|
||||
|
||||
else {
|
||||
return max;
|
||||
}
|
||||
}
|
||||
|
||||
void RunwayTakeoff::reset()
|
||||
{
|
||||
_initialized = false;
|
||||
|
|
|
@ -85,6 +85,7 @@ public:
|
|||
bool resetIntegrators();
|
||||
float getMinAirspeedScaling() { return _min_airspeed_scaling; };
|
||||
float getMinPitch(float sp_min, float climbout_min, float min);
|
||||
float getMaxPitch(float max);
|
||||
|
||||
void reset();
|
||||
|
||||
|
@ -107,6 +108,7 @@ private:
|
|||
control::BlockParamFloat _runway_takeoff_nav_alt;
|
||||
control::BlockParamFloat _runway_takeoff_throttle;
|
||||
control::BlockParamFloat _runway_takeoff_pitch_sp;
|
||||
control::BlockParamFloat _runway_takeoff_max_pitch;
|
||||
control::BlockParamFloat _airspeed_min;
|
||||
control::BlockParamFloat _climbout_diff;
|
||||
|
||||
|
|
|
@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
|
|||
PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
|
||||
|
||||
/**
|
||||
* Pitch setpoint during runway takeoff.
|
||||
* Pitch setpoint during taxi / before takeoff airspeed is reached.
|
||||
* A taildragger with stearable wheel might need to pitch up
|
||||
* a little to keep it's wheel on the ground before airspeed
|
||||
* to takeoff is reached.
|
||||
|
@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
|
|||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
|
||||
|
||||
/**
|
||||
* Max pitch during takeoff.
|
||||
* Fixed-wing settings are used if set to 0. Note that there is also a minimum
|
||||
* pitch of 10 degrees during takeoff, so this must be larger if set.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 60.0
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
|
||||
|
|
|
@ -1382,7 +1382,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
_mavlink_fd);
|
||||
|
||||
// update tecs
|
||||
float takeoff_pitch_max_deg = _parameters.pitch_limit_max;
|
||||
float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);
|
||||
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
|
||||
|
||||
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
|
||||
|
|
Loading…
Reference in New Issue