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

AP_TECS: sync TECS with plane4.0

This commit is contained in:
Andrew Tridgell 2020-05-11 18:14:48 +10:00
parent 8647cc9e72
commit 820729186a
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_TECS/AP_TECS.cpp
View File

@ -248,7 +248,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @Bitmask: 0:GliderOnly
// @User: Advanced
AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0),
AP_GROUPEND
};
@ -911,7 +911,7 @@ void AP_TECS::_update_pitch(void)
void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
{
// Initialise states and variables if DT > 1 second or in climbout
if (_DT > 1.0f)
if (_DT > 1.0f || _need_reset)
{
_integTHR_state = 0.0f;
_integSEB_state = 0.0f;
@ -927,6 +927,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_flags.reached_speed_takeoff = false;
_DT = 0.1f; // when first starting TECS, use a
// small time constant
_need_reset = false;
}
else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)
{
@ -1067,6 +1068,13 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_PITCHminf = MAX(_land_pitch_min, _PITCHminf);
}
if (_landing.is_flaring()) {
// ensure we don't violate the limits for flare pitch
if (_land_pitch_max != 0) {
_PITCHmaxf = MIN(_land_pitch_max, _PITCHmaxf);
}
}
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
// we have reached our target speed in takeoff, allow for

10
libraries/AP_TECS/AP_TECS.h
View File

@ -116,7 +116,12 @@ public:
void use_synthetic_airspeed(void) {
_use_synthetic_airspeed_once = true;
}
// reset on next loop
void reset(void) override {
_need_reset = true;
}
// this supports the TECS_* user settable parameters
static const struct AP_Param::GroupInfo var_info[];
@ -320,6 +325,9 @@ private:
float _land_pitch_min = -90;
// need to reset on next loop
bool _need_reset;
// internal variables to be logged
struct {
float SKE_weighting;