aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_TECS: added TECS_PITCH_MAX and TECS_PITCH_MIN

Browse Source 
this allows the TECS controlled pitch limits to be smaller than the
FBWA limits. It is common for a human pilot to want a bit more
discretion over pitch than you want the automatic controller to use
This commit is contained in:
Andrew Tridgell 2014-08-06 11:46:01 +10:00
parent 781c5bc5dd
commit 7c4d31c481
2 changed files with 36 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_TECS/AP_TECS.cpp
View File

@ -133,6 +133,22 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("LAND_SPDWGT", 14, AP_TECS, _spdWeightLand, 1.0f),
// @Param: PITCH_MAX
// @DisplayName: Maximum pitch in auto flight
// @Description: This controls maximum pitch up in automatic throttle modes. If this is set to zero then LIM_PITCH_MAX is used instead. The purpose of this parameter is to allow the use of a smaller pitch range when in automatic flight than what is used in FBWA mode.
// @Range: 0 45
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("PITCH_MAX", 15, AP_TECS, _pitch_max, 0),
// @Param: PITCH_MIN
// @DisplayName: Minimum pitch in auto flight
// @Description: This controls minimum pitch in automatic throttle modes. If this is set to zero then LIM_PITCH_MIN is used instead. The purpose of this parameter is to allow the use of a smaller pitch range when in automatic flight than what is used in FBWA mode. Note that TECS_PITCH_MIN should be a negative number.
// @Range: -45 0
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("PITCH_MIN", 16, AP_TECS, _pitch_min, 0),
AP_GROUPEND
};
@ -656,8 +672,24 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_EAS_dem = EAS_dem_cm * 0.01f;
_THRmaxf = aparm.throttle_max * 0.01f;
_THRminf = aparm.throttle_min * 0.01f;
_PITCHmaxf = 0.000174533f * aparm.pitch_limit_max_cd;
_PITCHminf = 0.000174533f * aparm.pitch_limit_min_cd;
// work out the maximum and minimum pitch
// if TECS_PITCH_{MAX,MIN} isn't set then use
// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
// larger than LIM_PITCH_{MAX,MIN}
if (_pitch_max <= 0) {
_PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
} else {
_PITCHmaxf = min(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
}
if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
} else {
_PITCHminf = max(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
}
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
_flight_stage = flight_stage;
// initialise selected states and variables if DT > 1 second or in climbout

2
libraries/AP_TECS/AP_TECS.h
View File

@ -120,6 +120,8 @@ private:
AP_Float _spdWeightLand;
AP_Float _landThrottle;
AP_Float _landAirspeed;
AP_Int8 _pitch_max;
AP_Int8 _pitch_min;
// throttle demand in the range from 0.0 to 1.0
float _throttle_dem;